Positioning method and system based on laser point cloud map

Abstract

The invention provides a positioning method and system based on a laser point cloud map. The method comprises the following steps: acquiring laser point cloud sequence data, IMU sequence data and RTK pose sequence data of an actual site based on a laser radar sensor, an IMU sensor and an RTK moving station; the laser point cloud sequence data, the IMU sequence data and the RTK pose sequence data are fused, and a laser point cloud map is constructed through multi-round optimization; marking and segmenting the laser point cloud map; and according to the segmented laser point cloud map, fusing the real-time laser point cloud data of the vehicle, the IMU data and the RTK pose data to obtain real-time positioning information of the vehicle. The method comprises the steps of laser radar point cloud collection, mapping based on the SLAM technology, laser point cloud marking, site high-precision map generation and U3D modeling of the site high-precision map, and high-precision positioning of a vehicle in a scene without RTK signals is achieved.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving, and more specifically, to a positioning method and system based on a laser point cloud map. Background Art

[0002] With the development of the robot and autonomous driving industries, various high-level assisted driving and low-speed autonomous driving have gradually become closer to life. Analogous to the similar low-speed, closed operating environment, the current driving training industry has also explored a new development direction. By introducing autonomous driving into the driving training industry, the driving training industry will surely usher in a profound change. In this way, while streamlining the composition of driving school personnel, using robots and autonomous driving technology to achieve teaching and training will surely better realize the intelligent development of driving schools and better guarantee driving school teaching.

[0003] Currently, in the field of smart driving schools, there are a large number of indoor scenes such as occlusion. In this case, high-precision RTK will have weak or no signal, and cannot effectively perform positioning, thus failing to achieve the universality of smart driving schools. Summary of the invention

[0004] In order to solve the problem that accurate positioning cannot be achieved in a scenario without RTK signal coverage, the present invention provides a positioning method and system based on a laser point cloud map.

[0005] According to a first aspect of the present invention, a positioning method based on a laser point cloud map is provided, comprising:

[0006] Enhance the actual site of the driving school based on reflective stickers;

[0007] Based on the laser radar sensor, IMU sensor and RTK mobile station, the laser point cloud sequence data, IMU sequence data and RTK pose sequence data of the actual site are obtained;

[0008] The laser point cloud sequence data, the IMU sequence data and the RTK pose sequence data are integrated to construct a laser point cloud map through multiple rounds of optimization;

[0009] Annotating the laser point cloud map, and segmenting the annotated laser point cloud map;

[0010] According to the segmented laser point cloud map, the laser point cloud data, IMU data and RTK posture data scanned by the vehicle in real time are fused to obtain the real-time positioning information of the vehicle.

[0011] Based on the above technical solution, the present invention can also make the following improvements.

[0012] Optionally, the actual site of the driving school is enhanced based on the reflective stickers, including:

[0013] For ground marking lines and ground guide lines in the site to be mapped, reflective stickers are placed at the corners of the ground marking lines and ground guide lines;

[0014] For the storage lines in the site to be mapped, reflective stickers are placed at the inner corners of the storage lines, including the storage lines for reverse parking, the parking lines for side parking, and the edge lines for right-angle turns;

[0015] For the S-curve of the storage location line in the site to be mapped, reflective stickers are placed along the inner sides of both sides of the S-curve.

[0016] Optionally, the step of acquiring laser point cloud sequence data, IMU sequence data and RTK pose sequence data of an actual site based on a laser radar sensor, an IMU sensor and an RTK mobile station includes calibrating external parameters of the laser radar sensor;

[0017] The calibrating the external parameters of the laser radar sensor includes:

[0018] Obtain laser point cloud sequence data and RTK pose sequence data collected by the vehicle along the preset route;

[0019] Preprocessing the laser point cloud sequence data, and performing time synchronization alignment on the preprocessed laser point cloud sequence data and the RTK pose sequence data;

[0020] For the aligned laser point cloud sequence data and the RTK pose sequence data, transforming the laser point cloud sequence data based on the initial external parameters of the laser radar sensor to obtain transformed laser point cloud sequence data;

[0021] Calculating the alignment error between the transformed laser point cloud sequence data and the RTK pose sequence data;

[0022] The alignment error is compared with a preset error threshold, and if the alignment error is greater than the preset error threshold, the external parameters of the laser radar sensor are adjusted to iteratively solve the alignment error between the transformed laser point cloud sequence data and the RTK pose sequence data;

[0023] The external parameters of the laser radar sensor are obtained by continuously adjusting the external parameters of the laser radar sensor until the calculated alignment error is less than or equal to a preset error threshold. The external parameters of the laser radar sensor include a translation vector and a rotation matrix.

[0024] Optionally, fusing the laser point cloud sequence data, the IMU sequence data and the RTK pose sequence data to construct a laser point cloud map through multiple rounds of optimization includes:

[0025] Building a laser odometer system according to the laser point cloud sequence data and the IMU sequence data, and filtering laser point cloud key frames from the laser point cloud sequence data based on the laser odometer system;

[0026] Perform interpolation alignment based on the timestamp of the laser point cloud key frame and the RTK pose data;

[0027] Based on the aligned laser point cloud keyframe and RTK pose data, a graph optimization system with the laser point cloud keyframe pose as the vertex is constructed, and the RTK pose absolute observation constraints and the laser odometer system inter-frame relative observation constraints are added to perform the first round of optimization on the laser point cloud keyframe;

[0028] According to the aligned laser point cloud keyframes and RTK pose data, loop detection is performed based on Euclidean distance, and corresponding loop pairings are screened and saved;

[0029] The laser point cloud key frame pose after the first round of optimization is used as the optimization vertex, loop pairing is added as a relative constraint, and the RTK pose absolute observation constraint and inter-frame relative observation constraint that eliminate outliers are added. The laser point cloud key frame is optimized for the second round to obtain the laser point cloud key frame pose after the second round of optimization.

[0030] The laser point cloud map is constructed based on the laser point cloud key frame pose after the second round of optimization.

[0031] Optionally, filtering laser point cloud key frames from the laser point cloud sequence data based on the laser odometer system includes:

[0032] If the current frame is the first frame in the laser point cloud sequence data, the current frame is used as a key frame;

[0033] If the current frame is not the first frame in the laser point cloud sequence data, register the current frame with the local map to obtain the pose of the current frame;

[0034] Calculate the distance between the pose of the current frame and the pose of the previous key frame. If the distance is greater than a preset distance threshold, the current frame is used as the next key frame of the previous key frame.

[0035] According to the pose of the current frame and the pose of the previous frame, the pose of the next frame is obtained based on the uniform motion model;

[0036] The position and posture of the next frame are continuously judged to determine whether the next frame is a key frame, until all key frames in the laser point cloud sequence data are found.

[0037] Optionally, constructing a laser point cloud map based on the laser point cloud key frame pose after the second round of optimization includes:

[0038] The laser point cloud key frame poses after the second round of optimization are spliced ​​according to different areas to form a complete laser point cloud map.

[0039] Optionally, the labeling of the laser point cloud map and segmenting of the labeled laser point cloud map include:

[0040] Using the cloudcompare tool, the corner points, S curves and electronic fences in the subject in the laser point cloud map are marked, coordinate conversion is performed based on the point cloud annotation text, a json file with longitude and latitude information is generated, and a 3D model of the site is generated based on Unity3D;

[0041] The annotated laser point cloud map is divided into different areas.

[0042] Optionally, the laser point cloud data, IMU data and RTK posture data scanned by the vehicle in real time are fused according to the segmented laser point cloud map to obtain the real-time positioning information of the vehicle, including:

[0043] When the vehicle runs to a position where the RTK signal is normal and stops, an initial grid search is performed based on the vehicle's RTK posture to initialize the laser positioning.

[0044] An iterative error Kalman filter is constructed based on the translation vector t and rotation matrix R of the lidar sensor, the vehicle speed v, the zero bias ba of the accelerometer, the zero bias bg of the gyroscope, and the gravity acceleration g as state variables, where the accelerometer and the gyroscope are IMU sensors;

[0045] The laser scanning point cloud data of the vehicle is registered to the laser point cloud map using the NDT registration algorithm, and the registration residual is calculated;

[0046] Inferring the next state based on the registration residual and the IMU data of the vehicle, and updating the state of the Kalman filter;

[0047] The state is iteratively updated based on the Kalman filter until the registration residual is less than or equal to the preset registration error, and the real-time positioning information of the vehicle is obtained.

[0048] Optionally, the laser point cloud data, IMU data and RTK pose data scanned by the vehicle in real time are fused to obtain the real-time positioning information of the vehicle, which also includes:

[0049] During the process of positioning the vehicle, the real-time positioning information of the vehicle is detected for abnormalities. If the real-time positioning information of the vehicle is abnormal, the laser positioning output is reinitialized according to the RTK posture of the vehicle.

[0050] According to a second aspect of the present invention, there is provided a positioning system based on a laser point cloud map, comprising:

[0051] An acquisition module, used for acquiring laser point cloud sequence data, IMU sequence data and RTK pose sequence data of an actual site based on a laser radar sensor, an IMU sensor and an RTK mobile station, wherein the actual site is enhanced based on reflective stickers;

[0052] A map construction module, used to fuse the laser point cloud sequence data, IMU sequence data and RTK pose sequence data, and construct a laser point cloud map through multiple rounds of optimization;

[0053] A labeling module, used for labeling the laser point cloud map and segmenting the labeled laser point cloud map;

[0054] The positioning module is used to fuse the laser point cloud data, IMU data and RTK posture data scanned by the vehicle in real time according to the segmented laser point cloud map to obtain the real-time positioning information of the vehicle.

[0055] The present invention provides a positioning method and system based on laser point cloud map, which obtains laser point cloud sequence data, IMU sequence data and RTK pose sequence data of the actual site based on laser radar sensor, IMU sensor and RTK mobile station; fuses the laser point cloud sequence data, IMU sequence data and RTK pose sequence data, and constructs a laser point cloud map through multiple rounds of optimization; annotates and segments the laser point cloud map; and according to the laser point cloud map after segmentation, fuses the real-time laser point cloud data, IMU data and RTK pose data of the vehicle to obtain the real-time positioning information of the vehicle. The present invention includes laser radar point cloud acquisition, mapping based on SLAM technology, annotating laser point cloud, generating high-precision site map, and using U3D to model the high-precision site map, so as to realize high-precision positioning of vehicles in scenes without RTK signals. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 A flow chart of a positioning method based on a laser point cloud map provided by the present invention;

[0057] Figure 2 An overall flow chart of a positioning method based on a laser point cloud map provided by the present invention;

[0058] Figure 3 A schematic diagram of collecting LiDAR and RTK data by driving around.

[0059] Figure 4 This is the flow chart of external parameter calibration for LiDAR sensor;

[0060] Figure 5 Build a flow chart for laser point cloud maps;

[0061] Figure 6 This is the laser positioning flow chart;

[0062] Figure 7 This is a schematic diagram of parking accuracy;

[0063] Figure 8 A structural block diagram of a positioning system based on a laser point cloud map provided by the present invention. DETAILED DESCRIPTION

[0064] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention. In addition, the technical features in the various embodiments or single embodiments provided by the present invention can be arbitrarily combined with each other to form a feasible technical solution. This combination is not subject to the constraints of the sequence of steps and / or the structural composition mode, but must be based on the ability of ordinary technicians in this field to achieve. When the combination of technical solutions is contradictory or cannot be achieved, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0065] In order to achieve high-precision mapping and positioning in scenarios where RTK (Real-time kinematic) signals cannot be covered, the technology used in this invention is laser point cloud mapping, using Unit y 3D site modeling enables unified positioning of vehicles regardless of normal or abnormal RTK scenarios.

[0066] The present invention collects laser point cloud of the actual site of the driving school, and performs key frame sampling, point cloud voxel filtering, dynamic point cloud culling, key point cloud upsampling, etc. on the laser point cloud to obtain high-quality PCD point cloud data, which is used as the base map, and the initial position relationship between the mapping origin and RTK is retained.

[0067] Based on the existing high-quality base map point cloud, the laser point cloud output by real-time scanning is highly accurately registered with the base map point cloud and its key frames, so as to output the accurate position of the real-time vehicle in the point cloud base map, and then convert it into longitude and latitude as the positioning data of the robot and unmanned vehicle system.

[0068] Figure 1 A flow chart of a positioning method based on a laser point cloud map provided by the present invention, such as Figure 1 and Figure 2 As shown, the method includes:

[0069] Step 1: Enhance the actual site of the driving school based on reflective stickers.

[0070] It is understandable that in order to make the data collected at the actual site clearer, first of all, the actual site is enhanced based on reflective stickers. Specifically, for the actual site, observe all ground marking lines, ground guide lines, and storage lines in the site that needs to be mapped. Based on the ground marking lines and guide lines, reflective stickers are placed at the corners; based on the storage lines, such as reversing into the warehouse, side parking, and right-angle turns, reflective stickers are placed at the inner corners; for S-curves, reflective stickers need to be placed along the inner sides of both sides, and the size of the reflective stickers is required to be 3cmx3cm.

[0071] Step 2: Based on the lidar sensor, IMU sensor and RTK mobile station, the laser point cloud sequence data, IMU sequence data and RTK pose sequence data of the actual site are obtained.

[0072] It is understandable that after the actual site is enhanced based on the reflective stickers, the laser point cloud sequence data, IMU sequence data and RTK pose sequence data of the actual site are collected based on the lidar sensor, IMU sensor and RTK mobile station.

[0073] Before using the lidar sensor, IMU sensor and RTK mobile station to collect laser point cloud sequence data, IMU sequence data and RTK pose sequence data of the actual site, it is necessary to calibrate the external parameters of the lidar sensor, where the external parameters of the lidar sensor include the translation vector and the rotation matrix.

[0074] Exemplarily, calibrating the external parameters of the laser radar sensor includes:

[0075] Obtain laser point cloud sequence data and RTK pose sequence data collected by the vehicle along the preset route;

[0076] Preprocessing the laser point cloud sequence data, and performing time synchronization alignment on the preprocessed laser point cloud sequence data and the RTK pose sequence data;

[0077] For the aligned laser point cloud sequence data and the RTK pose sequence data, transforming the laser point cloud sequence data based on the initial external parameters of the laser radar sensor to obtain transformed laser point cloud sequence data;

[0078] Calculating the alignment error between the transformed laser point cloud sequence data and the RTK pose sequence data;

[0079] The alignment error is compared with a preset error threshold, and if the alignment error is greater than the preset error threshold, the external parameters of the laser radar sensor are adjusted to iteratively solve the alignment error between the transformed laser point cloud sequence data and the RTK pose sequence data;

[0080] The external parameters of the laser radar sensor are obtained by continuously adjusting the external parameters of the laser radar sensor until the calculated alignment error is less than or equal to a preset error threshold. The external parameters of the laser radar sensor include a translation vector and a rotation matrix.

[0081] It is understandable that the process of calibrating the external parameters of the lidar sensor can be seen in Figure 3 First, collect data. Specifically, use a vehicle to collect LiDAR (laser point cloud data) and RTK data in an 8-shaped route. Use the vehicle to collect laser point cloud data and RTK data for a period of time to form laser point cloud sequence data and RTK pose sequence data. In the scenario covered by RTK signal, the collected RTK data is usually accurate. Therefore, the external parameters of the lidar sensor can be calibrated with reference to the RTK data.

[0082] The process of calibrating the external parameters of the lidar sensor can be found in Figure 4 , perform preprocessing such as denoising and filtering on the collected laser point cloud sequence data, and then time synchronize the preprocessed laser point cloud sequence data and RTK pose sequence data. Next, use the known rough external parameters (understood as initial external parameters) to preliminarily align the laser point cloud sequence data with the RTK pose sequence data. Specifically, according to the initial external parameters of the lidar sensor, the laser point cloud sequence data is converted to obtain the converted laser point cloud sequence data, and the alignment error between the converted laser point cloud sequence data and the RTK pose sequence data is calculated. According to the alignment error, the external parameters of the lidar sensor are adjusted until the alignment error between the transformed laser point cloud sequence data and the RTK pose sequence data is less than the threshold, and the external parameters of the lidar sensor at this time are obtained.

[0083] Based on the calibrated lidar sensor, IMU sensor and RTK mobile station, the laser point cloud sequence data, IMU sequence data and RTK pose sequence data of the actual site are obtained.

[0084] Step 3: Fuse the laser point cloud sequence data, IMU sequence data and RTK pose sequence data, and construct a laser point cloud map through multiple rounds of optimization.

[0085] Among them, see Figure 5 , the acquired laser point cloud sequence data, IMU sequence data and RTK pose sequence data are fused to realize laser point cloud mapping.

[0086] For example, the laser point cloud sequence data, the IMU sequence data and the RTK pose sequence data are fused, and a laser point cloud map is constructed through multiple rounds of optimization, including:

[0087] Building a laser odometer system according to the laser point cloud sequence data and the IMU sequence data, and filtering laser point cloud key frames from the laser point cloud sequence data based on the laser odometer system;

[0088] Perform interpolation alignment based on the timestamp of the laser point cloud key frame and the RTK pose data;

[0089] Based on the aligned laser point cloud keyframe and RTK pose data, a graph optimization system with the laser point cloud keyframe pose as the vertex is constructed, and the RTK pose absolute observation constraints and the laser odometer system inter-frame relative observation constraints are added to perform the first round of optimization on the laser point cloud keyframe;

[0090] According to the aligned laser point cloud keyframes and RTK pose data, loop detection is performed based on Euclidean distance, and corresponding loop pairings are screened and saved;

[0091] The laser point cloud key frame pose after the first round of optimization is used as the optimization vertex, loop pairing is added as a relative constraint, and the RTK pose absolute observation constraint and inter-frame relative observation constraint that eliminate outliers are added. The laser point cloud key frame is optimized for the second round to obtain the laser point cloud key frame pose after the second round of optimization.

[0092] The laser point cloud map is constructed based on the laser point cloud key frame pose after the second round of optimization.

[0093] Specifically, based on the collected laser point cloud sequence data and IMU data, a laser odometer system is constructed. At the same time, the laser odometer system selects key frame point clouds from the laser point cloud sequence data, and interpolates and aligns the key frame point clouds based on the timestamp and RTK data to reduce the subsequent calculation amount. Selecting laser point cloud key frames from laser point cloud sequence data includes:

[0094] If the current frame is the first frame in the laser point cloud sequence data, the current frame is used as a key frame;

[0095] If the current frame is not the first frame in the laser point cloud sequence data, register the current frame with the local map to obtain the pose of the current frame;

[0096] Calculate the distance between the pose of the current frame and the pose of the previous key frame. If the distance is greater than a preset distance threshold, the current frame is used as the next key frame of the previous key frame.

[0097] According to the pose of the current frame and the pose of the previous frame, the pose of the next frame is obtained based on the uniform motion model;

[0098] The position and posture of the next frame are continuously judged to determine whether the next frame is a key frame, until all key frames in the laser point cloud sequence data are found.

[0099] After filtering out the keyframe point cloud from the laser point cloud sequence data, interpolation alignment is performed based on the timestamp and RTK data of the keyframe point cloud. A graph optimization system with the keyframe point cloud pose as the vertex is constructed, and the RTK pose absolute observation constraints and the laser odometer inter-frame relative observation constraints are added. The laser point cloud keyframe is optimized in the first round and the first round of optimization results are obtained. Then, loop detection based on Euclidean distance is performed on the keyframe point cloud, and the corresponding loop pairing is filtered and saved. The keyframe pose after the first round of optimization is used as the optimization vertex, and the loop pairing is added as a relative constraint. The RTK observation and inter-frame observation constraints that remove outliers are added, and the second round of optimization is performed to obtain the final optimized keyframe pose. Finally, the point cloud map is spliced ​​based on the optimized keyframe pose to obtain the laser point cloud map.

[0100] Step 4: annotate the laser point cloud map, and segment the annotated laser point cloud map.

[0101] It is understandable that for the constructed laser point cloud map, point cloud annotation is performed based on the stitched point cloud map and point cloud enhancement effect, corner points, S curves and electronic fences within the subject are extracted based on the cloudcompare tool, coordinate conversion is performed based on the point cloud annotation text, a json file with longitude and latitude information is generated, and a 3D model of the site is generated based on Unity3D for display.

[0102] The annotated laser point cloud map is segmented according to different areas, and the segmented laser point cloud map can be used for laser positioning.

[0103] Step 5: According to the segmented laser point cloud map, the laser point cloud data, IMU data and RTK posture data scanned by the vehicle in real time are integrated to obtain the real-time positioning information of the vehicle.

[0104] Exemplarily, the laser point cloud data, IMU data and RTK pose data scanned by the vehicle in real time are fused according to the segmented laser point cloud map to obtain the real-time positioning information of the vehicle, including:

[0105] When the vehicle runs to a position where the RTK signal is normal and stops, an initial grid search is performed based on the vehicle's RTK posture to initialize the laser positioning.

[0106] An iterative error Kalman filter is constructed based on the translation vector t and rotation matrix R of the lidar sensor, the vehicle speed v, the zero bias ba of the accelerometer, the zero bias bg of the gyroscope, and the gravity acceleration g as state variables, where the accelerometer and the gyroscope are IMU sensors;

[0107] The laser point cloud data scanned by the vehicle in real time is registered with the laser point cloud map using the NDT registration algorithm, and the registration residual is calculated;

[0108] Inferring the next state based on the registration residual and the IMU data of the vehicle, and updating the state of the Kalman filter;

[0109] The state is iteratively updated based on the Kalman filter until the registration residual is less than or equal to the preset registration error, and the real-time positioning information of the vehicle is obtained.

[0110] Specifically, the real-time position of the vehicle is located based on the laser point cloud map constructed in step 3. Generally, RTK data is accurate. The process of locating the real-time position of the vehicle through the constructed laser point cloud map can be found in Figure 6 Therefore, an initial grid search is first performed based on the vehicle's RTK pose to initialize laser positioning. Then, an iterative error Kalman filter is constructed based on the translation vector t and rotation matrix R of the lidar sensor, the vehicle's speed v, the accelerometer's zero bias ba, the gyroscope's zero bias bg, and the gravity acceleration g as state variables.

[0111] The NDT registration algorithm is used to register the real-time point cloud data of the scanned vehicle to the laser point cloud map, and the registration residual is calculated. The next state is inferred based on the registration residual and the vehicle's IMU data, and the state of the Kalman filter is updated based on the inferred next state. The state is continuously iterated and updated through the Kalman filter until the registration error is less than the error threshold, and the real-time positioning information of the vehicle is obtained.

[0112] During the process of positioning the vehicle, the real-time positioning information of the vehicle is detected for abnormalities. If the real-time positioning information of the vehicle is abnormal, the RTK position of the vehicle is reinitialized to output the laser positioning.

[0113] The parking accuracy of the vehicle can be seen by using the method provided by the present invention. Figure 7 After sufficient testing, the average longitudinal deviation of the fixed-point parking accuracy of this method is 2.8cm, the average lateral deviation is 1.1cm, and the overall lateral and longitudinal deviations are within 3cm.

[0114] Positioning repeatability accuracy is shown in Table 1:

[0115] Table 1 Positioning repeatability accuracy

[0116] serial number x(m) y(m) yaw(rad) 1 2.47 -31.55 1.5993 2 2.44 -31.56 1.60679 3 2.44 -31.58 1.60779 4 2.44 -31.56 1.60735 5 2.43 -31.55 1.61102 6 2.41 -31.55 1.61338 7 2.41 -31.55 1.61369 8 2.42 -31.56 1.61422 9 2.42 -31.57 1.61419 10 2.41 -31.55 1.61459 average value 2.429 -31.557 1.610232 Error mean 0.014 0.007 Radians: 0.00393 / Degrees: 0.23

[0117] After sufficient testing, the positioning repeatability accuracy of this method is 1.4cm on the x-axis, 0.7cm on the y-axis, and 0.23 degrees on the heading angle.

[0118] See also Figure 8 , a positioning system based on a laser point cloud map provided by the present invention, comprising:

[0119] An acquisition module 801 is used to acquire laser point cloud sequence data, IMU sequence data and RTK pose sequence data of an actual site based on a laser radar sensor, an IMU sensor and an RTK mobile station, wherein the actual site is enhanced based on reflective stickers;

[0120] A map construction module 802 is used to fuse the laser point cloud sequence data, the IMU sequence data and the RTK pose sequence data, and construct a laser point cloud map through multiple rounds of optimization;

[0121] A labeling module 803 is used to label the laser point cloud map and segment the labeled laser point cloud map;

[0122] The positioning module 804 is used to fuse the real-time laser point cloud data, IMU data and RTK posture data of the vehicle according to the segmented laser point cloud map to obtain the real-time positioning information of the vehicle.

[0123] It can be understood that the positioning system based on laser point cloud map provided by the present invention corresponds to the positioning method based on laser point cloud map provided in the aforementioned embodiments. The relevant technical features of the positioning system based on laser point cloud map can refer to the relevant technical features of the positioning method based on laser point cloud map, which will not be repeated here.

[0124] The embodiment of the present invention provides a positioning method and system based on laser point cloud map, which obtains laser point cloud sequence data, IMU sequence data and RTK pose sequence data of the actual site based on laser radar sensor, IMU sensor and RTK mobile station; fuses the laser point cloud sequence data, IMU sequence data and RTK pose sequence data, and constructs a laser point cloud map through multiple rounds of optimization; annotates and segments the laser point cloud map; according to the segmented laser point cloud map, the real-time laser point cloud data, IMU data and RTK pose data of the vehicle are fused to obtain the real-time positioning information of the vehicle. The present invention includes laser radar point cloud acquisition, mapping based on SLAM technology, annotating laser point cloud, generating high-precision site map, using U3D to model the high-precision site map, and realizing high-precision positioning of vehicles in scenes without RTK signals.

[0125] It should be noted that in the above embodiments, the description of each embodiment has its own emphasis, and for parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0126] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0127] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded computer, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0128] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0129] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0130] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0131] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A positioning method based on laser point cloud map, characterized in that: include: Enhance the actual site of the driving school based on reflective stickers; Based on the laser radar sensor, IMU sensor and RTK mobile station, the laser point cloud sequence data, IMU sequence data and RTK pose sequence data of the actual site are obtained; The laser point cloud sequence data, the IMU sequence data and the RTK pose sequence data are integrated to construct a laser point cloud map through multiple rounds of optimization; Annotating the laser point cloud map, and segmenting the annotated laser point cloud map; According to the segmented laser point cloud map, the laser point cloud data, IMU data and RTK posture data scanned by the vehicle in real time are fused to obtain the real-time positioning information of the vehicle.

2. The positioning method based on laser point cloud map according to claim 1, characterized in that: The method of enhancing the actual site of the driving school based on the reflective stickers includes: For ground marking lines and ground guide lines in the site to be mapped, reflective stickers are placed at the corners of the ground marking lines and ground guide lines; For the storage lines in the site to be mapped, reflective stickers are placed at the inner corners of the storage lines, including the storage lines for reverse parking, the parking lines for side parking, and the edge lines for right-angle turns; For the S-curve of the storage location line in the site to be mapped, reflective stickers are placed along the inner sides of both sides of the S-curve.

3. The positioning method based on laser point cloud map according to claim 1, characterized in that: The laser point cloud sequence data, IMU sequence data and RTK pose sequence data of the actual site are obtained based on the laser radar sensor, the IMU sensor and the RTK mobile station, which includes calibrating the external parameters of the laser radar sensor; The calibrating the external parameters of the laser radar sensor includes: Obtain laser point cloud sequence data and RTK pose sequence data collected by the vehicle along the preset route; Preprocessing the laser point cloud sequence data, and performing time synchronization alignment on the preprocessed laser point cloud sequence data and the RTK pose sequence data; For the aligned laser point cloud sequence data and the RTK pose sequence data, transforming the laser point cloud sequence data based on the initial external parameters of the laser radar sensor to obtain transformed laser point cloud sequence data; Calculating the alignment error between the transformed laser point cloud sequence data and the RTK pose sequence data; The alignment error is compared with a preset error threshold, and if the alignment error is greater than the preset error threshold, the external parameters of the laser radar sensor are adjusted to iteratively solve the alignment error between the transformed laser point cloud sequence data and the RTK pose sequence data; The external parameters of the laser radar sensor are obtained by continuously adjusting the external parameters of the laser radar sensor until the calculated alignment error is less than or equal to a preset error threshold. The external parameters of the laser radar sensor include a translation vector and a rotation matrix.

4. The positioning method based on laser point cloud map according to claim 1, characterized in that: The laser point cloud sequence data, the IMU sequence data and the RTK pose sequence data are fused to construct a laser point cloud map through multiple rounds of optimization, including: Building a laser odometer system according to the laser point cloud sequence data and the IMU sequence data, and filtering laser point cloud key frames from the laser point cloud sequence data based on the laser odometer system; Perform interpolation alignment based on the timestamp of the laser point cloud key frame and the RTK pose data; Based on the aligned laser point cloud keyframe and RTK pose data, a graph optimization system with the laser point cloud keyframe pose as the vertex is constructed, and the RTK pose absolute observation constraints and the laser odometer system inter-frame relative observation constraints are added to perform the first round of optimization on the laser point cloud keyframe; According to the aligned laser point cloud keyframes and RTK pose data, loop detection is performed based on Euclidean distance, and corresponding loop pairings are screened and saved; The laser point cloud key frame pose after the first round of optimization is used as the optimization vertex, loop pairing is added as a relative constraint, and the RTK pose absolute observation constraint and inter-frame relative observation constraint that eliminate outliers are added. The laser point cloud key frame is optimized for the second round to obtain the laser point cloud key frame pose after the second round of optimization. The laser point cloud map is constructed based on the laser point cloud key frame pose after the second round of optimization.

5. The positioning method based on laser point cloud map according to claim 4 is characterized in that: Filtering laser point cloud key frames from the laser point cloud sequence data based on the laser odometer system includes: If the current frame is the first frame in the laser point cloud sequence data, the current frame is used as a key frame; If the current frame is not the first frame in the laser point cloud sequence data, register the current frame with the local map to obtain the pose of the current frame; Calculate the distance between the pose of the current frame and the pose of the previous key frame. If the distance is greater than a preset distance threshold, the current frame is used as the next key frame of the previous key frame. According to the pose of the current frame and the pose of the previous frame, the pose of the next frame is obtained based on the uniform motion model; The position and posture of the next frame are continuously judged to determine whether the next frame is a key frame, until all key frames in the laser point cloud sequence data are found.

6. The positioning method based on laser point cloud map according to claim 4 is characterized in that: The laser point cloud map is constructed based on the laser point cloud key frame pose after the second round of optimization, including: The laser point cloud key frame poses after the second round of optimization are spliced ​​according to different areas to form a complete laser point cloud map.

7. The positioning method based on laser point cloud map according to claim 1, characterized in that: The step of labeling the laser point cloud map and segmenting the labeled laser point cloud map includes: Using the cloudcompare tool, the corner points, S curves and electronic fences in the subject in the laser point cloud map are marked, coordinate conversion is performed based on the point cloud annotation text, a json file with longitude and latitude information is generated, and a 3D model of the site is generated based on Unity3D; The annotated laser point cloud map is divided into different areas.

8. The positioning method based on laser point cloud map according to claim 1, characterized in that: According to the segmented laser point cloud map, the laser point cloud data, IMU data and RTK posture data scanned by the vehicle in real time are integrated to obtain the real-time positioning information of the vehicle, including: When the vehicle runs to a position where the RTK signal is normal and stops, an initial grid search is performed based on the vehicle's RTK posture to initialize the laser positioning. An iterative error Kalman filter is constructed based on the translation vector t and rotation matrix R of the lidar sensor, the vehicle speed v, the zero bias ba of the accelerometer, the zero bias bg of the gyroscope, and the gravity acceleration g as state variables, where the accelerometer and the gyroscope are IMU sensors; The laser point cloud data scanned by the vehicle in real time is registered with the laser point cloud map using the NDT registration algorithm, and the registration residual is calculated; Inferring the next state based on the registration residual and the IMU data of the vehicle, and updating the state of the Kalman filter; The state is iteratively updated based on the Kalman filter until the registration residual is less than or equal to the preset registration error, and the real-time positioning information of the vehicle is obtained.

9. The positioning method based on laser point cloud map according to claim 8, characterized in that: The laser point cloud data, IMU data and RTK posture data scanned by the vehicle in real time are integrated to obtain the real-time positioning information of the vehicle, including: In the process of positioning the vehicle, the real-time positioning information of the vehicle is detected for abnormalities. If the real-time positioning information of the vehicle is abnormal, the laser positioning output is reinitialized according to the RTK posture of the vehicle.

10. A positioning system based on laser point cloud map, characterized in that: include: An acquisition module, used for acquiring laser point cloud sequence data, IMU sequence data and RTK pose sequence data of an actual site based on a laser radar sensor, an IMU sensor and an RTK mobile station, wherein the actual site is enhanced based on reflective stickers; A map construction module, used to fuse the laser point cloud sequence data, IMU sequence data and RTK pose sequence data, and construct a laser point cloud map through multiple rounds of optimization; A labeling module, used for labeling the laser point cloud map and segmenting the labeled laser point cloud map; The positioning module is used to fuse the laser point cloud data, IMU data and RTK posture data scanned by the vehicle in real time according to the segmented laser point cloud map to obtain the real-time positioning information of the vehicle.

Images