A multi-source data fusion positioning method based on low earth orbit satellite assistance

Abstract

The application discloses a kind of multi-source data fusion positioning methods based on low-orbit satellite auxiliary, utilize the three-dimensional space information of vehicle surrounding environment, vehicle data obtained by LiDAR, IMU and ODO installed in vehicle;Vehicle data measured by IMU and ODO is pre-integrated calculation, and the motion estimation information of vehicle is obtained;Laser point cloud data is motion compensated using motion estimation information, and the relative pose information of vehicle is calculated after feature extraction and matching of the compensated laser point cloud data;Global position information of vehicle in complex scene such as urban canyon is obtained using low-orbit satellite, the global position information is fused with motion estimation information and relative pose information, and high-precision positioning of vehicle in urban canyon type scene is realized by graph optimization method.

Description

Technical Field

[0001] This invention relates to the field of high-precision positioning technology for intelligent vehicles, specifically to a multi-source data fusion positioning method based on low-orbit satellite assistance. Background Technology

[0002] In recent years, with the rapid development of intelligent vehicles and autonomous driving technologies, high-precision positioning has become one of the core technologies in intelligent driving systems. Accurate vehicle positioning information is fundamental to subsequent functions such as environmental perception, path planning, and decision-making control, and is also extremely important for ensuring the safety and reliability of intelligent vehicle operation. Existing intelligent vehicle positioning technologies generally fall into two categories: single-sensor and multi-sensor data fusion methods.

[0003] Single sensors include GNSS, LiDAR, inertial measurement units (IMU), and visual cameras. GNSS can provide stable positioning information in open scenes, but its stability drops significantly in urban canyons or other areas where signals are easily obstructed. LiDAR can generate high-resolution environmental maps, but it is expensive and performs poorly in inclement weather. Although IMUs can acquire vehicle dynamic information at high frequency and infer the pose during driving from a given initial pose, they also accumulate errors over time. Visual cameras can provide high-precision positioning information in well-lit environments, but are limited by changes in lighting and the capabilities of computer image processing.

[0004] To address these issues, multi-sensor data fusion positioning technology has gradually become a research hotspot. By fusing data from multiple sources such as GNSS, IMU, LiDAR, and vision, the shortcomings of using a single sensor for positioning can be effectively compensated. However, in semi-enclosed scenarios like urban canyons, traditional multi-sensor data fusion methods still suffer from insufficient GNSS signals and difficulties in extracting environmental features, resulting in positioning accuracy that fails to meet the actual driving needs of intelligent vehicles. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this application proposes a multi-source data fusion positioning method based on low-orbit satellite assistance. By utilizing low-orbit satellite, multi-sensor data fusion, and graph optimization techniques, it significantly improves the high-precision positioning capability of intelligent vehicles in complex environments such as urban canyons.

[0006] The technical solution adopted in this invention is as follows:

[0007] A multi-source data fusion positioning method based on low-Earth orbit satellite assistance includes the following steps:

[0008] Step 1: Use the LiDAR, IMU and ODO installed in the vehicle to acquire three-dimensional spatial information of the surrounding environment and vehicle data;

[0009] Step 2: Perform pre-integration calculations on the vehicle data obtained from IMU and ODO measurements to obtain vehicle motion estimation information;

[0010] Step 3: Use motion estimation information to perform motion compensation on the laser point cloud data, extract and match features from the compensated laser point cloud data, and calculate the relative pose information of the vehicle.

[0011] Step 4: Use low-orbit satellites to obtain the vehicle's global position information in complex urban canyon scenarios. Then, fuse this global position information with motion estimation information and relative pose information, and use graph optimization to achieve high-precision positioning of the vehicle in urban canyon scenarios.

[0012] Furthermore, the IMU measures the vehicle's acceleration and angular velocity information in real time.

[0013] Furthermore, the ODO is installed on the wheel, and the wheel rotation angle and wheel speed are measured to calculate the vehicle's displacement and speed.

[0014] Furthermore, the LiDAR scans the vehicle's surrounding environment in real time to acquire high-resolution laser point cloud data, providing three-dimensional spatial information about the vehicle's surrounding environment.

[0015] Furthermore, the pre-integration calculation includes: performing pre-integration calculations on the acceleration and angular velocity measured by the IMU and the displacement and velocity measured by the ODO respectively; fusing the pre-integration results of the IMU and ODO using the EKF algorithm to obtain vehicle motion estimation information, which is denoted as... Including the vehicle's current location P k Speed ​​V k and posture R k .

[0016] Furthermore, the motion compensation is divided into translational compensation and rotational compensation. First, the vehicle's current position P is used. k Translation compensation is performed on the laser point cloud, and then the vehicle's attitude R is used. k Rotation compensation is applied to the laser point cloud.

[0017] Furthermore, the formulas for displacement compensation and rotation compensation are expressed as follows:

[0018] P compensated =P original -P k

[0019] P compensated =R k ·P original

[0020] In the formula, P compensatedThese are the compensated laser point cloud coordinates, P original These are the original coordinates of the laser point cloud, P. k This is the vehicle's current location, R. k It's an attitude.

[0021] Furthermore, the method for feature extraction and matching of laser point cloud data is as follows:

[0022] Take a point in the laser point cloud, and let the set of points in its neighborhood be denoted as . Calculate the curvature of the point by calculating the average neighborhood distance of the point cloud.

[0023] Based on the curvature of each point in the point cloud and the threshold k th By comparing the points, the points are divided into corner points, planar points, and edge points, and a set of feature points composed of corner points and edge points is extracted.

[0024] Based on the feature point matching error in the laser point cloud at the current moment and the previous moment, the rigid transformation between the two frames of laser point cloud data is calculated, and the relative pose of the vehicle is obtained.

[0025] Furthermore, during the graph optimization process, the global position information P provided by low-orbit satellites is used. leo As a global constraint, it is fused with the vehicle's motion estimation information and relative pose information to construct the objective function. The vehicle's pose estimation is optimized by minimizing the observation error, denoted as:

[0026]

[0027] In the formula, It is the predicted state vector, K k It is the Kalman gain, Z k These are observed values. It is a nonlinear observation function.

[0028] Furthermore, the formula for calculating observation error is as follows:

[0029]

[0030] In the formula, P ti and R ti These are the vehicle's position and attitude during the graph optimization process, P leo It is global position information provided by low-Earth orbit satellites, R imu The attitude information is provided by the IMU.

[0031] The beneficial effects of this invention are:

[0032] (1) This invention effectively integrates LiDAR, IMU, ODO and low-orbit satellite data, making full use of the advantages of each sensor, overcoming the limitations of a single sensor in complex scenarios such as urban canyons, and exhibiting higher positioning accuracy and robustness.

[0033] (2) This invention effectively eliminates the dynamic error of laser point cloud data caused by vehicle movement through pre-integration and dynamic compensation of IMU and ODO, providing more accurate environmental information for the calculation of relative pose.

[0034] (3) The present invention uses graph optimization technology to optimize multi-source data in real time, providing high-precision position, speed and attitude estimation of vehicles, ensuring that intelligent vehicles can drive autonomously in complex scenarios such as urban canyons.

[0035] (4) This invention is particularly suitable for urban canyon scenarios. It combines the high-frequency transit characteristics of low-orbit satellites, the high-precision spatial perception capability of iDAR, and the high-frequency motion estimation capability of IMU / ODO, enabling intelligent vehicles to drive stably in semi-enclosed urban canyon environments. Attached Figure Description

[0036] Figure 1 This is a flowchart of the low-orbit satellite-assisted intelligent vehicle multi-source fusion positioning method applicable to urban canyons as described in this invention;

[0037] Figure 2 This is a schematic diagram of the vehicle LiDAR, IMU, and ODO installation in this invention. Detailed Implementation

[0038] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the invention.

[0039] This invention proposes a low-orbit satellite-assisted multi-source fusion positioning method for intelligent vehicles suitable for urban canyons. Utilizing multi-sensor data fusion and graph optimization techniques, it significantly improves the high-precision positioning capability of intelligent vehicles in complex environments such as urban canyons. The method mainly includes a data acquisition stage, a data processing stage, a global positioning stage, and a data fusion stage.

[0040] During the data acquisition phase, LiDAR, IMU, and ODO are installed on the vehicle. The LiDAR scans the vehicle's surrounding environment in real time to acquire high-resolution laser point cloud data, providing three-dimensional spatial information about the vehicle's surroundings. The IMU is installed next to the LiDAR to measure the vehicle's acceleration and angular velocity in real time, estimating the vehicle's dynamic state. The ODO is installed on the wheels to measure the vehicle's speed and mileage in real time, providing necessary data support for motion estimation.

[0041] In the data processing stage, the measurement data from the IMU and ODO are first pre-integrated to calculate the vehicle's motion information. Combined with the vehicle's initial pose, motion estimation information such as displacement and rotation angle of the vehicle within a certain time period is calculated. Subsequently, motion compensation is performed on the laser point cloud data using this motion estimation information to eliminate dynamic errors caused by vehicle motion. The compensated laser point cloud data is then used for feature extraction and matching to calculate the vehicle's relative pose information.

[0042] In the global positioning phase, densely distributed low-orbit satellites are used to acquire stable global position information of vehicles in complex urban canyon scenarios, providing global constraints for subsequent positioning calculations.

[0043] In the data fusion stage, graph optimization technology is used to input the vehicle's global position information, motion estimation information, and relative pose information into the optimization framework, achieving high-precision real-time positioning of the vehicle in complex urban canyon-like scenarios. The specific steps of this invention are as follows:

[0044] Step 1: Use the LiDAR, IMU and ODO installed in the vehicle to acquire three-dimensional spatial information of the surrounding environment and vehicle data.

[0045] like Figure 2 As shown, a LiDAR is installed on the top of the vehicle, and an IMU is installed next to the LiDAR, ensuring that the IMU's position does not interfere with the LiDAR's scanning range. Meanwhile, ODOs are installed on the vehicle's wheels.

[0046] An IMU installed next to the LiDAR is used to measure the vehicle's acceleration and angular velocity in real time to calculate the vehicle's dynamic state.

[0047] ODOs installed on the wheels measure the vehicle's speed and mileage in real time.

[0048] LiDAR scans the vehicle's surroundings in real time, acquiring high-resolution laser point cloud data to provide three-dimensional spatial information about the vehicle's environment. While the vehicle is in motion, LiDAR scans the surrounding environment to acquire laser point cloud data for complex scenes such as urban canyons where the vehicle is located.

[0049] Specifically, LiDAR calculates the distance between a vehicle and a target object by emitting a laser beam and determining the time it takes for the laser to reflect back. By measuring the direction of the laser beam's emission and the reflection time, LiDAR can accurately generate corresponding laser point cloud data in three-dimensional space. The LiDAR calculation of the distance to the target object is shown in the following formula:

[0050]

[0051] In the formula, D is the distance to the target object, c is the speed of light, and t is the time required for the laser beam to travel from emission to reflection and return.

[0052] Specifically, LiDAR scanners use rotation and oscillation to allow the laser beam to cover different angles of the vehicle's surroundings. During each scan cycle, the LiDAR emits a laser beam and records the reflection time of the returning laser at each point in time. Each reflected laser beam generates a three-dimensional coordinate point (x, y, z), forming a point cloud. Through multiple scans, LiDAR can comprehensively cover the vehicle's driving area, generating a large amount of laser point cloud data and uploading it to a computing platform for processing.

[0053] Specifically, an IMU and an ODO are used to measure the vehicle's motion information. The IMU primarily measures the vehicle's linear acceleration (a) along three coordinate axes using its built-in three accelerometers and three gyroscopes. x ,a y ,a z ) and rotational angular velocity (ω) x ,ω y ,ω z ODO estimates vehicle displacement and speed by measuring wheel rotation; the wheel angle is closely related to the tire radius. Vehicle data measured by IMU and ODO is uploaded to the computing platform in real time, awaiting further data processing.

[0054] Step 2: The computing platform performs pre-integration calculations on the vehicle data obtained from IMU and ODO measurements to calculate the real-time motion information of the vehicle. Then, combined with the initial pose of the vehicle, it calculates the motion estimation information such as displacement and rotation angle of the vehicle within a certain time period.

[0055] Specifically, the pre-integration calculation method is as follows:

[0056] The computing platform obtains the vehicle's velocity and attitude information by integrating the acceleration and angular velocity measured by the IMU. Assuming the vehicle's velocity at a certain moment t0 is v(t0), and the vehicle's acceleration during the current time interval is a(t), the vehicle's velocity can be obtained by integrating the acceleration, as shown in equation (2):

[0057]

[0058] In the formula, Δt is the time interval.

[0059] Assuming the initial attitude of the vehicle at time t0 is R(t0), the attitude of the vehicle at time t0+Δt, R(t0+Δt), can be obtained by integrating the angular velocity, as shown in equation (3):

[0060] R(t0+Δt)=R(t0)·exp(ω(t0)·Δt) (3)

[0061] In the formula, ω(t0) is the angular velocity of the vehicle at time t0.

[0062] The computing platform simultaneously receives the wheel rotation angle and wheel speed measured by ODO to calculate the vehicle's displacement and velocity. Assuming the radius of the vehicle's wheel is r and the wheel rotation angle is θ, the vehicle's displacement d can be obtained from equation (4):

[0063] d=r·θ (4)

[0064] The vehicle's speed v is determined by the wheel rotation speed ω. wheel The calculation is shown in equation (5):

[0065] v=r·ω wheel (5)

[0066] Since the IMU and ODO provide real-time vehicle motion information (the IMU provides acceleration and angular velocity, while the ODO provides displacement and velocity), they are affected by different types of errors during actual vehicle operation (such as accumulated errors of the IMU and tire slippage of the ODO). Therefore, the pre-integration results of the IMU and ODO are fused using the EKF (Extended Kalman Filter) algorithm to obtain more accurate vehicle motion estimation information.

[0067] The vehicle's state vector X k Including position, velocity, and attitude, denoted as:

[0068]

[0069] In the formula, P k Indicates the vehicle's position, V k P represents the speed of the vehicle. k Indicates the vehicle's posture.

[0070] Combining the pre-integral data from the IMU and ODO, the vehicle's state is predicted. The state prediction equation is expressed as follows:

[0071]

[0072] In the formula, For the predicted vehicle status, The state transition function represents the calculation of vehicle motion using the acceleration and angular velocity of the IMU and the displacement and velocity of the ODO, μ. k The input quantity includes measurement data from the IMU and ODO.

[0073] The vehicle state is updated by fusing the displacement and velocity observations from the ODO with the predicted state. The update equation is shown in equation (8):

[0074]

[0075] In the formula, For the updated state vector (high-precision position, velocity, and attitude information of the vehicle), Z k For ODO observations (displacement and velocity), K is a nonlinear observation function (mapping the state to the observation space). k Kalman gain, used to balance the difference between predicted and observed values, is calculated using the following formula:

[0076]

[0077] In the formula, H is the predicted state covariance matrix. k Let Jacobian matrix be the equation of observation. To observe the noise covariance matrix.

[0078] After updating the state, the covariance matrix P also needs to be updated. k The covariance matrix reflects the accuracy of the current estimate. The updated covariance matrix takes into account the fusion of predicted and observed data, reducing the uncertainty of the estimate. The formula for updating the covariance matrix is ​​shown in equation (10):

[0079]

[0080] In the formula, I is the identity matrix.

[0081] This update process can further improve the accuracy of vehicle motion state estimation and effectively reduce the uncertainty caused by measurement errors and noise.

[0082] Step 3: Use motion estimation information to perform motion compensation on the laser point cloud data to eliminate dynamic errors caused by vehicle motion. Then, perform feature extraction and matching on the compensated laser point cloud data to calculate the vehicle's relative pose information.

[0083] Specifically, during autonomous driving, the laser point cloud data acquired by LiDAR is affected by factors such as vehicle acceleration and steering, resulting in translation and rotation. To address this issue, motion compensation for the laser point cloud data is performed using motion estimation information fused from EKF. The motion estimation information after EKF fusion is as follows: Including the vehicle's current location P k Speed ​​V k and posture R kMotion compensation consists of two steps: translation compensation and rotation compensation. First, the vehicle's current position P is used. k Translation compensation is performed on the laser point cloud, and then the vehicle's attitude R is used. k Rotation compensation is performed on the laser point cloud. The specific translation compensation and rotation compensation formulas are shown in equations (11) and (12):

[0084] P compensated =P original -P k (11)

[0085] P compensated =R k ·P original (12)

[0086] In the formula, P compensated These are the compensated laser point cloud coordinates, P original These are the original coordinates of the laser point cloud.

[0087] By using translation and rotation compensation, the displacement and orientation of the point cloud data were corrected, resulting in more accurate urban environmental data.

[0088] After motion compensation, feature extraction and matching are performed on the compensated laser point cloud data to calculate the relative pose of the vehicle. Specifically, a point p in the laser point cloud is selected, and its neighborhood set is set as N(p). The curvature k of the point is calculated by calculating the average neighborhood distance of the point cloud. The calculation formula is shown in equation (13):

[0089]

[0090] In the formula, r mean It is the average distance from point p to other points in the neighborhood of point p, which can be obtained by equation (14):

[0091]

[0092] In the formula, ||qp|| is the distance between neighboring point q and point p, and |N(p)| is the number of points in the neighborhood.

[0093] Determine whether a point is a feature point based on its curvature value in the point cloud, and set a threshold k. th Comparison, k th The median curvature of the current frame is given, and the standard deviation of curvature σ is calculated. k If k > k th +2σ k Mark this point as a corner point; if k <k th -2σ k Mark this point as a plane point; if k is in k thIf the point is nearby, it is marked as an edge point. Through this judgment process, a set of feature points composed of corner points and edge points is extracted from the point cloud. Then, based on the feature point matching error between the current and previous laser point cloud data, the rigid transformation between the two frames of laser point cloud data is accurately calculated, thereby obtaining the relative pose of the vehicle. The formula for calculating the feature point matching error is as follows:

[0094]

[0095] In the formula, and These are the corresponding feature points in the laser point cloud at the current and previous moments, respectively. This represents the distance between each pair of matched feature points.

[0096] The relative pose of the vehicle can be calculated using equations (16) and (17):

[0097] Δp=P current -P previous (16)

[0098]

[0099] In the formula, P current and P previous These represent the vehicle's current and previous positions, respectively; ΔP is the vehicle's displacement between adjacent time frames; R current and R previous These are the vehicle's attitude at the current time and the previous time, respectively, and Δp is the change in the vehicle's attitude between adjacent time frames.

[0100] Step 4: Use low-orbit satellites to obtain the vehicle's global position information in complex urban canyon scenarios. Then, fuse this global position information with motion estimation information and relative pose information, and use graph optimization to achieve high-precision positioning of the vehicle in urban canyon scenarios.

[0101] Specifically, leveraging the low orbital altitude and high robustness of low-Earth orbit satellites, in complex urban canyon-like scenarios, vehicles acquire their latitude, longitude, and altitude information in real time. This global position information is denoted as P. leo It can be obtained by trigonometric measurement, as shown in equation (18):

[0102] ||P i -P leo ||=ρ i (18)

[0103] In the formula, P i The known position of the i-th low-orbit satellite ([x i y i ,zi ]),ρ i It corresponds to the pseudorange (the deviation between the measured distance and the actual distance), ρ i The calculation formula is shown in equation (19):

[0104] ρ i =c·t i +δ i (19)

[0105] In the formula, c is the speed of light, and t is the speed of light. i δ represents the signal propagation time from low-Earth orbit satellite i to the vehicle. i This represents the pseudorange error.

[0106] After obtaining global position information from low-Earth orbit satellites, a graph optimization method is used to fuse it with the vehicle's motion estimation and relative pose information to achieve high-precision vehicle positioning in complex urban canyon scenarios. Specifically, precise pose information is obtained by establishing constraints and minimizing errors. First, the vehicle's state vector is defined as follows: It combines global positioning information from low-Earth orbit satellites, motion estimation information from IMU / ODO, and relative pose information of the vehicle, including the vehicle's position. speed and posture Recorded as:

[0107]

[0108] During the image optimization process, global position information P provided by low-orbit satellites leo As a global constraint, it is fused with the vehicle's motion estimation information and relative pose information to construct an objective function. This objective function optimizes the vehicle's pose estimation by minimizing the observation error. The formula for calculating the observation error is as follows:

[0109]

[0110] In the formula, P ti and R ti These are the vehicle's position and attitude during the graph optimization process, P leo It is global position information provided by low-Earth orbit satellites, R imu The attitude information is provided by the IMU.

[0111] Then, the state vector is optimized by minimizing the error function. After iterative updates, high-precision position, velocity, and attitude estimates of the vehicle are obtained. The optimized state vector is:

[0112]

[0113] In the formula, It is the predicted state vector, K k It is the Kalman gain, Z k These are observed values. It is a nonlinear observation function.

[0114] The final pose estimate of the vehicle is obtained based on the above steps. This not only demonstrates high positioning accuracy but also achieves robustness of the vehicle in complex urban canyon scenarios, ensuring stable operation of the vehicle in semi-enclosed urban environments with signal obstruction and dynamic changes.

[0115] The above embodiments are only used to illustrate the design concept and features of the present invention, and their purpose is to enable those skilled in the art to understand the content of the present invention and implement it accordingly. The protection scope of the present invention is not limited to the above embodiments. Therefore, all equivalent changes or modifications made based on the principles and design ideas disclosed in the present invention are within the protection scope of the present invention.

Claims

1. A multi-source data fusion positioning method based on low-Earth orbit satellite-assisted positioning, characterized in that, Includes the following steps: Step 1: Use the LiDAR, IMU and ODO installed in the vehicle to acquire three-dimensional spatial information of the surrounding environment and vehicle data; Step 2: Perform pre-integration calculations on the vehicle data obtained from IMU and ODO measurements to obtain vehicle motion estimation information; The pre-integration calculation includes: performing pre-integration calculations on the acceleration and angular velocity measured by the IMU and the displacement and velocity measured by the ODO respectively; fusing the pre-integration results of the IMU and ODO using the EKF algorithm to obtain vehicle motion estimation information, which is denoted as . This includes the vehicle's current location. ,speed and posture ; Step 3: Use motion estimation information to perform motion compensation on the laser point cloud data, extract and match features from the compensated laser point cloud data, and calculate the relative pose information of the vehicle. Step 4: Use low-orbit satellites to obtain the vehicle's global position information in complex urban canyon scenarios, fuse this global position information with motion estimation information and relative pose information, and use graph optimization methods to achieve high-precision positioning of the vehicle in urban canyon scenarios. During the image optimization process, global position information provided by low-orbit satellites will be used. As a global constraint, it is fused with the vehicle's motion estimation information and relative pose information to construct the objective function. The vehicle's pose estimation is optimized by minimizing the observation error, denoted as: ； In the formula, It is the predicted state vector. It is Kalman gain. These are observed values. It is a nonlinear observation function; The formula for calculating observation error is as follows: ； In the formula, and These refer to the vehicle's position and attitude during the graph optimization process. It is global position information provided by low-Earth orbit satellites. It is attitude information provided by the IMU.

2. The multi-source data fusion positioning method based on low-orbit satellite assistance according to claim 1, characterized in that, The IMU measures the vehicle's acceleration and angular velocity information in real time.

3. The multi-source data fusion positioning method based on low-Earth orbit satellite assistance according to claim 1, characterized in that, The ODO is installed on the wheel and measures the wheel angle and wheel speed to calculate the vehicle's displacement and speed.

4. The multi-source data fusion positioning method based on low-Earth orbit satellite-assisted positioning according to claim 1, characterized in that, The LiDAR scanner scans the vehicle's surroundings in real time, acquiring high-resolution laser point cloud data and providing three-dimensional spatial information about the vehicle's environment.

5. The multi-source data fusion positioning method based on low-Earth orbit satellite-assisted positioning according to claim 1, characterized in that, The motion compensation is divided into translation compensation and rotation compensation, first using the vehicle's current position. Translation compensation is applied to the laser point cloud, and then the vehicle's attitude is used. Rotation compensation is applied to the laser point cloud.

6. The multi-source data fusion positioning method based on low-Earth orbit satellite-assisted positioning according to claim 5, characterized in that, The formulas for displacement compensation and rotation compensation are expressed as follows: ； ； In the formula, These are the compensated laser point cloud coordinates. These are the original coordinates of the laser point cloud. This is the vehicle's current location. It's an attitude.

7. The multi-source data fusion positioning method based on low-Earth orbit satellite-assisted positioning according to claim 1, characterized in that, The method for feature extraction and matching of laser point cloud data is as follows: Take a point in the laser point cloud, and let the set of points in its neighborhood be denoted as . Calculate the curvature of the point by calculating the average neighborhood distance of the point cloud. Based on the curvature and threshold of each point in the point cloud By comparing the points, the points are divided into corner points, planar points, and edge points, and a set of feature points composed of corner points and edge points is extracted. Based on the feature point matching error in the laser point cloud at the current moment and the previous moment, the rigid transformation between the two frames of laser point cloud data is calculated, and the relative pose of the vehicle is obtained.

Images