Laser positioning system and method based on point cloud map

Abstract

The invention discloses a laser positioning system and method based on a point cloud map. The laser positioning system comprises a map point cloud generation module, a track plotting module, a laser initial positioning state estimation module, a laser matching module, a laser positioning state monitoring module and a laser repositioning module. According to the method, laser matching positioning is carried out based on the point cloud map, the matching target is map point cloud, and compared with inter-frame matching, accumulated errors of laser positioning can be reduced; the track plotting is carried out based on the IMU and the wheel speed meter, when the laser positioning state is normal, the track plotting result is corrected, and the accuracy of state estimation is improved; on the basis of IMU, a wheel speed meter and laser point cloud data, multiple input sources are provided for laser initial motion state estimation and repositioning state estimation, a state estimation result is output based on input source state decision, the estimation result can still be output under the condition that part of sensors or methods fail, the laser positioning stability is improved, and the application range is widened.

Description

Technical Field

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

[0002] Positioning technology plays a key role in autonomous driving. As a common vehicle-mounted sensor, LiDAR plays an important role in positioning. There are two common laser positioning methods: positioning based on laser SLAM and positioning based on point cloud maps. Among them, the positioning method based on laser SLAM can obtain the laser positioning result by matching two frames of adjacent point cloud data in laser SLAM, and then obtaining the odometer data. However, there is a problem of cumulative error in laser SLAM positioning. In the absence of global observation, the cumulative error may gradually increase, reducing the accuracy of positioning. The laser positioning method based on point cloud maps matches the target point cloud with the map point cloud. It is necessary to generate a point cloud map through a special mapping process before positioning. Based on this, the current laser point cloud and the map point cloud are used for matching to obtain the current vehicle's position and posture on the map. With the addition of map offset, the vehicle's positioning information can be obtained to reduce the impact of cumulative errors.

[0003] There are two main difficulties in laser positioning based on point cloud maps, namely the estimation of the initial positioning state and how to reposition when positioning is lost. During the laser positioning process, the initial positioning state is continuously updated. If the initial positioning state is not estimated or only relies on the laser point cloud for estimation, the subsequent matching with the map point cloud may fail due to excessive errors, resulting in positioning loss. Although the combination of IMU and wheel speed meter can perform track calculation, calculate the position and posture of the vehicle in a short time, and can be used to estimate the initial positioning state. However, if only IMU and wheel speed meter are used for long-term calculations, the cumulative error will gradually increase and the positioning accuracy will decrease. In addition, positioning may be lost during the laser positioning process, so it is necessary to continuously monitor the laser positioning state and reposition when the laser positioning is lost. Summary of the invention

[0004] The present invention provides a laser positioning system and method based on point cloud maps to overcome the above problems of existing laser positioning.

[0005] The present invention adopts the following technical solution:

[0006] A laser positioning system based on a point cloud map, comprising:

[0007] The map point cloud generation module generates map point clouds based on IMU and laser radar, and provides matching target point clouds for the laser matching module;

[0008] The dead reckoning module calculates the position and attitude based on the IMU and wheel speed meter, and provides input for the initial laser positioning state and re-positioning state estimation; when the laser positioning is normal, the dead reckoning result is corrected;

[0009] The laser initial positioning state estimation module estimates laser motion based on laser point cloud data, provides one input source, one input source based on the dead reckoning result, one input source based on the laser positioning corrected dead reckoning result, and one input source based on the laser positioning result at the previous moment, a total of four estimation results. According to the input source situation, the decision is made to output the laser initial positioning state estimation result;

[0010] The laser matching module matches the current laser point cloud with the map point cloud based on the initial positioning state estimation result to obtain the position and attitude information;

[0011] Laser positioning status monitoring module, used to monitor the laser positioning status in real time and analyze and evaluate the positioning results;

[0012] The laser repositioning module provides one input source based on the dead reckoning result, one input source based on the laser positioning corrected dead reckoning result, and one input source based on the laser positioning result at the previous moment, for a total of three estimation results. When the laser positioning state is lost, the laser repositioning state estimation result is obtained based on the input source situation, the current laser point cloud is converted, and a local search and match is performed with the map point cloud to output the repositioning result.

[0013] Furthermore, the above-mentioned map point cloud generation module integrates the three-axis acceleration and three-axis angular velocity information output by the IMU, obtains the position and posture change within the time interval of two consecutive frames of laser point clouds, and uses this as the initial value to perform inter-frame registration on the two frames of laser point clouds. After collecting the complete running trajectory, a complete map point cloud is generated for laser positioning.

[0014] Furthermore, the above-mentioned track calculation module uses the three-axis acceleration and three-axis angular velocity output by the IMU and the speed information output by the wheel speed meter to jointly perform track calculation to estimate the current position and attitude.

[0015] Furthermore, the above-mentioned laser motion estimation based on laser point cloud data requires obtaining the laser positioning results of the previous two moments, calculating the three-axis speed and three-axis angular velocity, and estimating the position and posture corresponding to the current moment according to the time interval between the previous moment and the current moment.

[0016] Furthermore, the laser positioning state monitoring module calculates a matching index based on the matching results of the current laser point cloud and the map point cloud, and obtains the laser positioning state according to a state judgment criterion.

[0017] Furthermore, the above-mentioned laser positioning status monitoring module specifically determines the laser positioning status based on the matching value, matching iteration number, matching time, speed estimation value, and angular velocity estimation value of the current laser point cloud and the map point cloud.

[0018] The present invention also provides a laser positioning method based on a point cloud map, comprising the above-mentioned laser positioning system based on a point cloud map, and the specific steps are as follows:

[0019] S10. Based on the installation requirements of IMU and wheel speed meter, they are installed at the corresponding positions of the vehicle. IMU provides three-axis acceleration and three-axis angular velocity, and wheel speed meter provides speed information, which is input into the dead reckoning module.

[0020] S20, installing a laser radar on the top of the vehicle, collecting point clouds around the vehicle, and inputting a single-frame point cloud as a current laser point cloud into a laser matching module;

[0021] S30, the dead reckoning module calculates the position and attitude information based on the IMU and the wheel speed meter, and provides input for the laser initial positioning state estimation module and the laser repositioning module;

[0022] S40, based on the states of the multiple input sources, the laser initial positioning state estimation module decides to output the laser initial positioning state estimation result;

[0023] S50, downsampling the current laser point cloud, and the laser matching module converts the current laser point cloud based on the initial positioning state estimation result, matches it with the map point cloud, and obtains the position and posture information;

[0024] S60, the laser positioning state monitoring module monitors the matching value, matching iteration number, matching time, speed estimation value, and angular velocity estimation value of the current laser point cloud and the map point cloud based on the laser matching result of step S50, and obtains the laser positioning state according to the state judgment criterion;

[0025] S70, when laser positioning is lost, the laser repositioning module obtains the laser repositioning state estimation result based on multiple input sources, converts the current laser point cloud, matches it with the map point cloud and calculates the matching state. If the matching state is normal, the repositioning is successful and the laser positioning continues;

[0026] S80, if the repositioning fails, rotate and translate the current laser point cloud within an appropriate range, continue to match it with the map point cloud, calculate the matching status, and if the matching status is normal, the repositioning is successful and laser positioning continues.

[0027] Furthermore, before performing laser positioning, the vehicle is used to collect maps based on the running route, and a map point cloud is generated based on the IMU and lidar data to provide a matching target point cloud for the laser matching module.

[0028] The above step S40 specifically performs laser motion estimation based on the laser point cloud data, providing an input source for the laser initial positioning state estimation module; the laser positioning results of the past two moments are recorded as X1 = [x1, y1, z1, α1, β1, γ1] T , X2=[x2, y2, z2, α2, β2, γ2] T , where x, y, z are the three-axis position coordinates, the coordinate origin is the map origin, α, β, γ are the roll angle, pitch angle and heading angle; motion estimation is performed based on the laser positioning result, and the current laser initial positioning state is estimated to be X3 = [x3, y3, z3, α3, β3, γ3] T ,in t1 is the time interval between the past two moments, and t2 is the time interval from the previous moment to the current moment; the dead reckoning result provides one input source, the dead reckoning result based on laser positioning correction provides one input source, and the laser positioning result of the previous moment provides one input source, for a total of four estimation results; when the laser positioning is normal, the dead reckoning result based on laser positioning correction is used first, and when the IMU state is abnormal, the laser motion estimation result is used; when the laser positioning is abnormal, the dead reckoning result is used, and if the standard deviation of the dead reckoning result is too large, the laser positioning result of the previous moment is used.

[0029] The above step S50 is specifically as follows: let the rotation matrix R = R γ R β R α ,in γ p is the estimated heading angle of the initial positioning state at the current moment, β p is the estimated value of the pitch angle of the initial positioning state at the current moment, α p is the estimated roll angle of the initial positioning state at the current moment; translation vector T = [x p ,y p , z p ] T , where the three-axis components are the three-axis coordinate estimates of the initial positioning state at the current moment. After converting the current laser point cloud, use the NDT or ICP algorithm to match it with the map point cloud to obtain the current position and attitude information.

[0030] It can be seen from the above description of the present invention that, compared with the prior art, the present invention has the following advantages:

[0031] 1. The present invention performs laser matching positioning based on point cloud maps, and the matching target is the map point cloud. Compared with inter-frame matching, it can reduce the cumulative error of laser positioning. Track calculation is performed based on IMU and wheel speed meter. When the laser positioning state is normal, the track calculation result is corrected to improve the accuracy of state estimation. Based on IMU, wheel speed meter and laser point cloud data, multiple input sources are provided for laser initial motion state estimation and repositioning state estimation. The state estimation result is output based on the input source state decision. In the case of failure of some sensors or methods, the estimation result can still be output, thereby improving the stability and application scope of laser positioning.

[0032] 2. The laser initial positioning state estimation module of the present invention is based on the laser motion estimation, track calculation results, laser positioning corrected track calculation results and the laser positioning results of the previous moment, a total of four estimation results. According to the state of the input source, the decision is made to output the initial positioning state estimation result, so that when some sensors or methods fail, the laser initial state can still be estimated, improving the stability and application scope of laser positioning; and the laser repositioning module also has three input sources, a total of three estimation results, obtains the laser repositioning state estimation result, converts the current laser point cloud, performs local search and matching with the map point cloud, and outputs the repositioning result. When the laser positioning is lost, repositioning can be performed according to the existing input source, and the point cloud matching range can be expanded by performing local search and matching with the map point cloud. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 It is a structural block diagram of the present invention.

[0034] Figure 2 It is a schematic diagram of the process of the present invention.

[0035] Figure 3 It is a schematic diagram of the matching process based on the initial laser positioning state estimation of the present invention.

[0036] Figure 4 It is a schematic diagram of the laser repositioning process of the present invention. DETAILED DESCRIPTION

[0037] Refer to the following Figure 1 The specific implementation of the present invention is described. In order to fully understand the present invention, many details are described below, but for those skilled in the art, the present invention can be implemented without these details. For well-known components, methods and processes, they are not described in detail below.

[0038] This embodiment provides a laser positioning system based on a point cloud map. Figure 1, including: a map point cloud generation module 10, a track calculation module 20, a laser initial positioning state estimation module 30, a laser matching module 40, a laser positioning state monitoring module 50 and a laser repositioning module 60.

[0039] The map point cloud generation module 10 generates a map point cloud based on the IMU and the laser radar, and provides a matching target point cloud for the laser matching module 40. Specifically, the map point cloud generation module integrates the three-axis acceleration and three-axis angular velocity information output by the IMU, obtains the position and attitude change within the time interval of two consecutive frames of laser point clouds, and uses this as the initial value to perform inter-frame registration on the two frames of laser point clouds. After collecting the complete running trajectory, a complete map point cloud is generated for laser positioning.

[0040] The dead reckoning module 20 provides input for the initial laser positioning state and repositioning state estimation based on the information of the position and attitude calculated by the IMU and the wheel speed meter; when the laser positioning is normal, the dead reckoning result is corrected. Among them, the IMU outputs three-axis acceleration and three-axis angular velocity, and the wheel speed meter outputs speed information, and the dead reckoning is performed together to estimate the current position and attitude.

[0041] The laser initial positioning state estimation module 30 performs laser motion estimation based on laser point cloud data, provides one input source, one input source based on track estimation results, one input source based on laser positioning corrected track estimation results, and one input source based on the laser positioning result at the previous moment, a total of four estimation results. According to the input source situation, the decision to output the laser initial positioning state estimation result is made.

[0042] To estimate laser motion based on laser point cloud data, it is necessary to obtain the laser positioning results of the previous two moments, calculate the three-axis velocity and three-axis angular velocity, and estimate the position and posture corresponding to the current moment based on the time interval between the previous moment and the current moment.

[0043] The laser matching module 40 matches the current laser point cloud with the map point cloud based on the initial positioning state estimation result to obtain the position and attitude information.

[0044] The laser positioning state monitoring module 50 is used to monitor the laser positioning state in real time and analyze and evaluate the positioning results. Specifically, the laser positioning state monitoring module 50 determines the laser positioning state based on the matching value, matching iteration number, matching time, speed estimation value, and angular velocity estimation value of the current laser point cloud and the map point cloud, calculates the matching index, and obtains the laser positioning state according to the state judgment criterion.

[0045] The laser repositioning module 60 provides one input source based on the track calculation result, one input source based on the laser positioning corrected track calculation result, and one input source based on the laser positioning result at the previous moment, for a total of three estimation results. When the laser positioning state is lost, the laser repositioning state estimation result is obtained based on the input source situation, the current laser point cloud is converted, and a local search and matching is performed with the map point cloud to output the repositioning result.

[0046] Reference Figure 2 The above-mentioned laser positioning system based on point cloud map, its positioning method comprises the following steps:

[0047] S10. Based on the installation requirements of IMU and wheel speed meter, they are installed at the corresponding positions of the vehicle. IMU provides three-axis acceleration and three-axis angular velocity, and wheel speed meter provides speed information, which is input into the dead reckoning module.

[0048] S20, installing a laser radar on the top of the vehicle, collecting point clouds around the vehicle, and inputting a single-frame point cloud as a current laser point cloud into a laser matching module.

[0049] S30, the dead reckoning module uses ESKF to calculate the position and attitude information based on the three-axis acceleration and three-axis angular velocity output of the IMU and the speed output of the wheel speed meter. Among them, the error state is predicted: δx′=Fδx, where δx is the error state variable and F is the linearized Jacobian matrix. Prediction covariance matrix: P'=FPF T +Q, where Q is related to noise. When laser positioning is normal, the positioning result can be used to update ESKF, correct the track calculation result, and provide input for the laser initial positioning state estimation module and the laser repositioning module. Error state update process:

[0050] K=P'H T (HP′H T +V) -1

[0051] δx=K(zh(x′))

[0052] P=(I-KH)P'

[0053] Where K is the Kalman gain, P' is the prediction covariance matrix, H is the Jacobian matrix of the observation equation relative to the error state, V is the covariance matrix of the observation noise, z is the observation data, h is the observation equation, and P is the corrected covariance matrix.

[0054] S40: Based on the states of the multiple input sources, the laser initial positioning state estimation module decides to output the laser initial positioning state estimation result.

[0055] Reference Figure 3, step S40 specifically performs laser motion estimation based on laser point cloud data, providing an input source for the laser initial positioning state estimation module; the laser positioning results of the past two moments are recorded as X1 = [x1, y1, z1, α1, β1, γ1] T , X2=[x2, y2, z2, α2, β2, γ2] T , where x, y, z are the three-axis position coordinates, the coordinate origin is the map origin, α, β, γ are the roll angle, pitch angle and heading angle; motion estimation is performed based on the laser positioning result, and the current laser initial positioning state is estimated to be X3 = [x3, y3, z3, α3, β3, γ3] T ,in t1 is the time interval between the past two moments, and t2 is the time interval from the previous moment to the current moment; the dead reckoning result provides one input source, the dead reckoning result based on laser positioning correction provides one input source, and the laser positioning result of the previous moment provides one input source, for a total of four estimation results; when the laser positioning is normal, the dead reckoning result based on laser positioning correction is used first, and when the IMU state is abnormal, the laser motion estimation result is used; when the laser positioning is abnormal, the dead reckoning result is used, and if the standard deviation of the dead reckoning result is too large, the laser positioning result of the previous moment is used.

[0056] S50, downsampling the current laser point cloud, and the laser matching module converts the current laser point cloud based on the initial positioning state estimation result, matches it with the map point cloud, and obtains the position and posture information;

[0057] Let the rotation matrix R = R γ R β R α ,in γ p is the estimated heading angle of the initial positioning state at the current moment, β p is the estimated value of the pitch angle of the initial positioning state at the current moment, α p is the estimated roll angle of the initial positioning state at the current moment; translation vector T = [x p ,y p , z p ] T , where the three-axis components are the three-axis coordinate estimates of the initial positioning state at the current moment. After converting the current laser point cloud, use the NDT or ICP algorithm to match it with the map point cloud to obtain the current position and attitude information.

[0058] S60, the laser positioning status monitoring module monitors the matching value, matching iteration number, matching time, speed estimation value, and angular velocity estimation value of the current laser point cloud and the map point cloud based on the laser matching result of step S50, and obtains the laser positioning status according to the status judgment criteria.

[0059] S70, when laser positioning is lost, refer to Figure 4 The laser relocation module is based on multiple input sources, of which the dead reckoning result provides one input source, the dead reckoning result based on laser positioning correction provides one input source, and the laser positioning result of the previous moment provides one input source, for a total of three estimation results. When the laser positioning is normal, the dead reckoning result of laser positioning correction is used first, otherwise the dead reckoning result is used. If the standard deviation of the dead reckoning result is too large, the laser positioning result of the previous moment is used for relocation. Based on the laser relocation state estimation result, the current laser point cloud is converted, matched with the map point cloud and the matching state is calculated. If the matching state is normal, the relocation is successful and laser positioning continues.

[0060] S80, if repositioning fails, rotate and translate the current laser point cloud within an appropriate range, for example, use the current heading angle to increase or decrease by 10° as the angle change boundary, rotate the current laser point cloud at intervals of 5°, and perform laser repositioning, use the current position to increase or decrease by 2m in the x and y axis directions as the position change boundary, translate the current laser point cloud at intervals of 40cm, and perform laser repositioning, continue matching with the map point cloud, calculate the matching status, if the matching status is normal, the repositioning is successful, and laser positioning continues.

[0061] Between the above steps S20 and S30, that is, before laser positioning is performed, map collection is first performed based on the running route, and a map point cloud is generated based on the IMU and laser radar data to provide a matching target point cloud for the laser matching module.

[0062] The above is only a specific implementation of the present invention, but the design concept of the present invention is not limited thereto. Any non-substantial changes to the present invention using this concept shall be deemed as an infringement of the protection scope of the present invention.

Claims

1. A laser positioning system based on point cloud map, characterized in that: include: The map point cloud generation module generates map point clouds based on IMU and laser radar, and provides matching target point clouds for the laser matching module; The dead reckoning module calculates the position and attitude based on the IMU and wheel speed meter, and provides input for the initial laser positioning state and re-positioning state estimation; when the laser positioning is normal, the dead reckoning result is corrected; The laser initial positioning state estimation module estimates laser motion based on laser point cloud data, provides one input source, one input source based on the dead reckoning result, one input source based on the laser positioning corrected dead reckoning result, and one input source based on the laser positioning result at the previous moment, a total of four estimation results. According to the input source situation, the decision is made to output the laser initial positioning state estimation result; The laser matching module matches the current laser point cloud with the map point cloud based on the initial positioning state estimation result to obtain the position and attitude information; Laser positioning status monitoring module, used to monitor the laser positioning status in real time and analyze and evaluate the positioning results; The laser repositioning module provides one input source based on the dead reckoning result, one input source based on the laser positioning corrected dead reckoning result, and one input source based on the laser positioning result at the previous moment, for a total of three estimation results. When the laser positioning state is lost, the laser repositioning state estimation result is obtained based on the input source situation, the current laser point cloud is converted, and a local search and match is performed with the map point cloud to output the repositioning result.

2. A laser positioning system based on a point cloud map as claimed in claim 1, characterized in that: The map point cloud generation module integrates the three-axis acceleration and three-axis angular velocity information output by the IMU, obtains the position and posture change within the time interval of two consecutive frames of laser point clouds, uses this as the initial value, performs inter-frame registration on the two frames of laser point clouds, and after collecting the complete running trajectory, generates a complete map point cloud for laser positioning.

3. A laser positioning system based on point cloud map as claimed in claim 1, characterized in that: The dead reckoning module uses the three-axis acceleration and three-axis angular velocity output by the IMU and the speed information output by the wheel speed meter to jointly perform dead reckoning and estimate the current position and attitude.

4. The laser positioning system based on point cloud map according to claim 1, characterized in that: The laser motion estimation based on laser point cloud data requires obtaining the laser positioning results of the previous two moments, calculating the three-axis speed and three-axis angular velocity, and estimating the position and posture corresponding to the current moment based on the time interval between the previous moment and the current moment.

5. The laser positioning system based on point cloud map according to claim 1, characterized in that: The laser positioning state monitoring module calculates the matching index based on the matching results of the current laser point cloud and the map point cloud, and obtains the laser positioning state according to the state judgment criterion.

6. A laser positioning system based on point cloud map as claimed in claim 5, characterized in that: The laser positioning status monitoring module specifically determines the laser positioning status based on the matching value, matching iteration number, matching time, speed estimation value, and angular velocity estimation value of the current laser point cloud and the map point cloud.

7. A laser positioning method based on a point cloud map, comprising a laser positioning system based on a point cloud map as claimed in claim 1, characterized in that: The specific steps are as follows: S10. Based on the installation requirements of IMU and wheel speed meter, they are installed at the corresponding positions of the vehicle. IMU provides three-axis acceleration and three-axis angular velocity, and wheel speed meter provides speed information, which is input into the dead reckoning module. S20, installing a laser radar on the top of the vehicle, collecting point clouds around the vehicle, and inputting a single-frame point cloud as a current laser point cloud into a laser matching module; S30, the dead reckoning module calculates the position and attitude information based on the IMU and the wheel speed meter, and provides input for the laser initial positioning state estimation module and the laser repositioning module; S40, based on the states of the multiple input sources, the laser initial positioning state estimation module decides to output the laser initial positioning state estimation result; S50, downsampling the current laser point cloud, and the laser matching module converts the current laser point cloud based on the initial positioning state estimation result, matches it with the map point cloud, and obtains the position and posture information; S60, the laser positioning state monitoring module monitors the matching value, matching iteration number, matching time, speed estimation value, and angular velocity estimation value of the current laser point cloud and the map point cloud based on the laser matching result of step S50, and obtains the laser positioning state according to the state judgment criterion; S70, when laser positioning is lost, the laser repositioning module obtains the laser repositioning state estimation result based on multiple input sources, converts the current laser point cloud, matches it with the map point cloud and calculates the matching state. If the matching state is normal, the repositioning is successful and the laser positioning continues; S80, if the repositioning fails, rotate and translate the current laser point cloud within an appropriate range, continue to match it with the map point cloud, calculate the matching status, and if the matching status is normal, the repositioning is successful and laser positioning continues.

8. The laser positioning method based on point cloud map according to claim 7, characterized in that: Before laser positioning, the vehicle is used to collect maps based on the running route, and a map point cloud is generated based on the IMU and lidar data to provide matching target point cloud for the laser matching module.

9. The laser positioning method based on point cloud map according to claim 7, characterized in that: The step S40 specifically performs laser motion estimation based on the laser point cloud data, providing an input source for the laser initial positioning state estimation module; the laser positioning results of the past two moments are recorded as X1=[x1, y1, z1, α1, β1, γ1] T , X2=[x2, y2, z2, α2, β2, γ2] T , where x, y, z are the three-axis position coordinates, the coordinate origin is the map origin, α, β, γ are the roll angle, pitch angle and heading angle; motion estimation is performed based on the laser positioning result, and the current laser initial positioning state is estimated to be X3 = [x3, y3, z3, α3, β3, γ3] T ,in t1 is the time interval between the past two moments, and t2 is the time interval from the previous moment to the current moment; the dead reckoning result provides one input source, the dead reckoning result based on laser positioning correction provides one input source, and the laser positioning result of the previous moment provides one input source, for a total of four estimation results; when the laser positioning is normal, the dead reckoning result based on laser positioning correction is used first, and when the IMU state is abnormal, the laser motion estimation result is used; when the laser positioning is abnormal, the dead reckoning result is used, and if the standard deviation of the dead reckoning result is too large, the laser positioning result of the previous moment is used.

10. The laser positioning method based on point cloud map according to claim 7, characterized in that: The step S50 is specifically as follows: let the rotation matrix R = R γ R β R α ,in γ p is the estimated heading angle of the initial positioning state at the current moment, β p is the estimated value of the pitch angle of the initial positioning state at the current moment, α p is the estimated roll angle of the initial positioning state at the current moment; translation vector T = [x p ,y p , z p ] T , where the three-axis components are the three-axis coordinate estimates of the initial positioning state at the current moment. After converting the current laser point cloud, use the NDT or ICP algorithm to match it with the map point cloud to obtain the current position and attitude information.

Images