A method and system for simultaneous positioning and mapping of laser radar

Abstract

The present invention provides a method and system for simultaneous positioning and mapping of a laser radar, the method comprising: obtaining ranging data of a carrier through an altimeter, and obtaining point cloud data of the carrier through a laser radar; solving the ranging data through an IMU to obtain external posture data of the carrier on a height channel; the external posture data includes the height of the carrier from the ground, the roll angle, and the pitch angle; solving the point cloud data through an IMU to obtain internal posture data of the carrier on a horizontal channel; the internal posture data includes the x position, y position, and heading angle of the carrier; performing posture estimation based on the external posture data and the internal posture data to obtain the posture data of the carrier. The present invention can solve the problem of reduced positioning accuracy of laser radar in a degraded environment.

Description

Technical Field

[0001] The present invention relates to the field of positioning and navigation technology, and in particular to a method and system for simultaneous positioning and mapping by a laser radar. Background Art

[0002] The simultaneous localization and mapping (SLAM) method based on LiDAR is one of the effective methods to solve the environmental positioning and perception problems of mobile robots. It can still ensure positioning in an environment without GNSS signals, and is more lighting stable and environmentally adaptable than the visual SLAM method. However, in degraded environments such as long tunnels and corridors with sparse geometric structures and large indoor scenes with similar elevation and direction structures, the LiDAR-based SLAM method will face the lack of constraints in certain directions, resulting in deviations in pose estimation in these directions, leading to failure of point cloud registration and ultimately reduced positioning and mapping accuracy. In response to the phenomenon that the lidar SLAM method will degrade in unstructured scenes, Masaki used the Fisher information matrix to determine the current degree of degradation, but this method can only determine whether it is degraded, but cannot obtain the corresponding degradation direction; JiZhang, Weikun Zhen and others proposed methods to analyze the degradation degree and degradation direction of the current lidar through perturbation models respectively. These two methods analyzed the degradation state, but did not actually solve the problems of reduced positioning accuracy and mapping deviation of the lidar in a degraded environment. In addition, these algorithms need to calculate the Jacobian matrix of the optimization equation to determine the degradation situation, which requires a large amount of calculation, high computing cost and is difficult to meet real-time requirements.

[0003] In the GNSS failure scenario, the inertial navigation system (IMU) can continuously measure the current position of the moving body by dead reckoning navigation. However, since the navigation information is obtained through integration, the positioning error will accumulate over time, so it is often used to assist other positioning methods to achieve multi-sensor fusion positioning. The LiDAR SLAM algorithms with LiDAR and IMU as the main sensors include the LOAM algorithm and the LeGO-LOAM algorithm, which use IMU to provide motion compensation for LiDAR SLAM. This method uses IMU to provide external motion compensation information for LiDAR SLAM algorithms, but does not solve the problem of failure of laser SLAM point cloud registration in degraded environments, and will result in degradation failure. There are also LIOM, LINS and LIO-SAM algorithms that tightly couple IMU in the LiDAR SLAM algorithm, but this type of IMU-assisted LiDAR SLAM positioning method does not consider the state estimation information of LiDAR in degraded environments, and does not analyze the degraded environment. Therefore, even if sensors such as IMU are added, the problem of failure of laser SLAM point cloud registration in degraded environments cannot be solved well, and only has weak degradation processing capabilities. Moreover, these algorithms mostly focus on the degradation in horizontal environments, and no effective solutions have been proposed for the degradation problem in elevation environments.

[0004] The prior art has the following disadvantages:

[0005] First, pure LiDAR SLAM algorithms will degenerate in scenarios with sparse structural features due to reduced constraints. In degraded scenarios, the point cloud registration results of the LiDAR SLAM method converge to incorrect poses, resulting in a decrease in the overall performance of LiDAR SLAM.

[0006] Second, the currently proposed degradation detection method only determines whether the lidar is currently in a degraded state and the current degradation direction, but does not solve problems such as the reduced positioning accuracy of the lidar in a degraded environment and deviations in mapping.

[0007] Third, the current fusion algorithms of lidar and other sensors do not analyze the state estimation of the lidar SLAM method in a degraded environment. Even if sensors such as IMU are added, they only have weak degradation processing capabilities. In addition, the current fusion algorithms mostly focus on the degradation in horizontal environments, and no effective solution has been proposed for the degradation problem in elevation environments.

[0008] Fourth, the degradation detection algorithm and the currently commonly used IMU+LiDAR fusion algorithms based on optimization and extended Kalman filtering require the calculation of the Jacobian matrix, which has a large amount of calculation, requires a high computing cost and is difficult to meet real-time requirements. Summary of the invention

[0009] The purpose of the present invention is to provide a method and system for simultaneous positioning and mapping of a laser radar, so as to solve the problem of failure of laser radar SLAM in a degraded environment.

[0010] In order to achieve the above-mentioned object, in a first aspect, an embodiment of the present invention provides a method for simultaneous positioning and mapping by a laser radar, the method comprising:

[0011] Acquiring ranging data of the carrier by means of an altimeter, and acquiring point cloud data of the carrier by means of a laser radar;

[0012] The distance measurement data is solved by the IMU to obtain the external posture data of the carrier on the height channel; the external posture data includes the height of the carrier from the ground, the roll angle and the pitch angle;

[0013] The point cloud data is solved by the IMU to obtain the internal posture data of the carrier on the horizontal channel; the internal posture data includes the x position, y position and heading angle of the carrier;

[0014] Perform posture estimation based on the external posture data and the internal posture data to obtain the posture data of the carrier.

[0015] Furthermore, the distance measurement data is solved by the IMU to obtain the external posture data of the carrier on the height channel, including:

[0016] The rolling angle and pitch angle of the carrier obtained by IMU calculation are used to perform height compensation on the ranging data to obtain the height of the carrier from the ground.

[0017] Furthermore, the roll angle and pitch angle of the carrier obtained by the IMU solution are used to perform height compensation on the ranging data to obtain the height of the carrier from the ground. Specifically, the following formula is used for correction calculation:

[0018]

[0019] in, Indicates the height of the carrier from the ground, l indicates the ranging information, Indicates the carrier roll angle calculated by IMU, Indicates the pitch angle of the carrier obtained by IMU solution.

[0020] Furthermore, the point cloud data is solved by the IMU to obtain the internal posture data of the carrier on the horizontal channel, including:

[0021] Extracting geometric feature points of the point cloud data, performing point cloud registration on the geometric feature points, and obtaining a decoupled point cloud pose;

[0022] Performing degradation estimation on the decoupled point cloud pose to obtain a re-expressed pose estimation result;

[0023] According to the re-expressed pose estimation result, the error state quantity at the previous moment is updated through ESKF tight coupling to obtain the error state quantity at the current moment.

[0024] Further, the error state quantity at the previous moment is updated by ESKF tight coupling according to the re-expressed pose estimation result to obtain the error state quantity at the current moment, including:

[0025] The angular velocity and acceleration of the carrier are obtained through the IMU, and the angular velocity and acceleration of the carrier are solved and integrated to obtain the main state of the system;

[0026] The error state of the system is estimated by using the IMU error covariance matrix, and the main state is superimposed on the error state to obtain the system state;

[0027] According to the re-expressed pose estimation result, the error state and the system state are updated to obtain the error state quantity at the current moment.

[0028] Furthermore, performing point cloud registration on the geometric feature points includes:

[0029] For the edge points in the geometric feature points, a pair of matching points with the shortest distance and not on the same laser beam are searched by the kd-tree method, and a constraint of minimum distance from point to line is established;

[0030] For the plane points in the geometric feature points, the kd-tree method is used to search for three matching points that are closest to each other and can form a triangle, and a constraint of minimum distance from point to plane is established;

[0031] Constructing a target equation according to the constraint of the minimum distance from the point to the line and the constraint of the minimum distance from the point to the surface;

[0032] The edge point is a point with the largest curvature among non-ground points, and the plane point is a point with the smallest curvature among non-ground points.

[0033] Furthermore, the constraint of establishing the minimum distance from point to line is specifically adopted by the following formula:

[0034]

[0035] Among them, p je and p le are all matching points, p ie is the edge point, k is the current moment, k-1 is the previous moment; p k,ie is the current time point p ie The three-dimensional coordinates in the laser radar coordinate system, pk-1,je is the previous time point p je The three-dimensional coordinates in the laser radar coordinate system, p k-1,le is the previous time point p le The three-dimensional coordinates in the lidar coordinate system.

[0036] The constraint of establishing the minimum distance from point to surface is specifically established by the following formula:

[0037]

[0038] Among them, p js 、p ls and p ms are all matching points, p is is a plane point, p k,is is the plane point p at the current moment is The three-dimensional coordinates in the laser radar coordinate system, p k-1,js is the matching point p at the previous moment js The three-dimensional coordinates in the laser radar coordinate system, p k-1,ls is the matching point p at the previous moment ls The three-dimensional coordinates in the laser radar coordinate system, p k-1,ms is the matching point p at the previous moment ms The three-dimensional coordinates in the lidar coordinate system.

[0039] The target equation is constructed by using the following formula:

[0040]

[0041] Among them, T * To construct the objective equation, is the pose change between the current moment and the previous moment.

[0042] Furthermore, the step of obtaining the point cloud data of the carrier by using a laser radar further includes:

[0043] The point cloud data is motion compensated by the IMU.

[0044] Furthermore, performing motion compensation on the point cloud data by using the IMU includes:

[0045] Calculate the pose of the IMU data at the current moment and the previous moment, and use the linear interpolation method to calculate the pose in the IMU coordinate system at any moment.

[0046] In a second aspect, the present invention provides a system for simultaneous positioning and mapping by a laser radar, the system comprising: an altimeter, a laser radar, an IMU and a posture estimation module, wherein:

[0047] The altimeter is used to obtain the ranging data of the carrier; the laser radar is used to obtain the point cloud data of the carrier;

[0048] The IMU includes a height channel solving module and a horizontal channel solving module, wherein the height channel solving module is used to solve the ranging data through the IMU to obtain the external posture data of the carrier on the height channel; the external posture data includes the height of the carrier from the ground, the roll angle and the pitch angle; the horizontal channel solving module is used to solve the point cloud data through the IMU to obtain the internal posture data of the carrier on the horizontal channel; the internal posture data includes the x position, y position and heading angle of the carrier;

[0049] The posture estimation module is used to perform posture estimation based on the external posture data and the internal posture data to obtain the posture data of the carrier.

[0050] Furthermore, the height channel solving module also includes a height compensation module for

[0051] The rolling angle and pitch angle of the carrier obtained by IMU calculation are used to perform height compensation on the ranging data to obtain the height of the carrier from the ground.

[0052] Furthermore, the horizontal channel solving module includes:

[0053] A feature extraction module is used to extract geometric feature points of the point cloud data, perform point cloud registration on the geometric feature points, and obtain a decoupled point cloud pose;

[0054] A degradation estimation module, used for performing degradation estimation on the decoupled point cloud pose to obtain a re-expressed pose estimation result;

[0055] The ESKF tight coupling module is used to update the error state quantity at the previous moment through ESKF tight coupling according to the re-expressed posture estimation result to obtain the error state quantity at the current moment.

[0056] Furthermore, the IMU also includes a motion compensation module for performing motion compensation on the point cloud data.

[0057] The present invention provides a method and system for simultaneous positioning and mapping of a laser radar, wherein the ranging data of a carrier is obtained by an altimeter, and the point cloud data of the carrier is obtained by a laser radar; the ranging data is solved by an IMU to obtain external posture data of the carrier on a height channel; the external posture data includes the height of the carrier from the ground, the roll angle, and the pitch angle; the point cloud data is solved by an IMU and a laser radar to obtain internal posture data of the carrier on a horizontal channel; the internal posture data includes the x position, y position, and heading angle of the carrier; the posture data of the carrier is obtained according to the external posture data and the internal posture data; the present invention can solve the problem of reduced positioning accuracy of the laser radar in a degraded environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 It is a schematic diagram of a flow chart of a method for simultaneous positioning and mapping of a laser radar provided in an embodiment of the present invention;

[0059] Figure 2 1 is a flow chart of step S13 of a method for simultaneous positioning and mapping by a laser radar provided in an embodiment of the present invention;

[0060] Figure 3 is a schematic diagram of a degraded environment constraint equation provided by an embodiment of the present invention;

[0061] Figure 4 is a flow chart of fusion positioning based on Error-State Kalman filtering provided by an embodiment of the present invention;

[0062] Figure 5 This is a system block diagram of a laser radar simultaneous positioning and mapping system according to an embodiment of the present invention;

[0063] Figure 6 1 is a system block diagram of a height channel solving module 31 of a laser radar simultaneous positioning and mapping system according to an embodiment of the present invention;

[0064] Figure 7 1 is a system block diagram of a horizontal channel solving module 32 of a laser radar simultaneous positioning and mapping system according to an embodiment of the present invention;

[0065] Figure 8 1 is a system block diagram of a horizontal channel motion compensation module 33 of a laser radar simultaneous positioning and mapping system according to an embodiment of the present invention. DETAILED DESCRIPTION

[0066] In order to make the purpose, technical scheme and beneficial effects of the present application clearer, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. Obviously, the embodiments described below are part of the embodiments of the present invention and are only used to illustrate the present invention, but are not used to limit the scope of the present invention. Based on the embodiments of 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.

[0067] In one embodiment, Figure 1 As shown, a method for simultaneous positioning and mapping of a laser radar is provided, the method comprising:

[0068] S11, obtaining ranging data of the carrier through an altimeter, and obtaining point cloud data of the carrier through a laser radar;

[0069] Because the point cloud data collected by the laser radar has the characteristics of huge data volume and noise interference, this embodiment first performs data downsampling, filtering to remove noise and ground segmentation preprocessing on the point cloud data collected by the laser radar.

[0070] In order to make the features of the point cloud data richer, this embodiment performs motion compensation on the point cloud data through IMU. Specifically, the pose of the IMU data at the current moment and the previous moment is calculated, and the pose in the IMU coordinate system at any moment is calculated using the linear interpolation method. That is, before the laser radar point cloud is matched, for a point cloud scanning time [k-1, k] at a moment, the pose of the IMU data at moments k-1 and k is calculated, where moment k is the current moment. With the advantage of the high frequency of IMU data, the pose of the IMU coordinate system at any moment in this moment is calculated using the linear interpolation method. This embodiment obtains the pose of the point cloud data in the world coordinate system at the current moment through calibration to remove the distorted part of the laser radar point cloud.

[0071] S12, solving the ranging data by IMU to obtain external posture data of the carrier on the height channel; the external posture data includes the height of the carrier from the ground, the roll angle and the pitch angle;

[0072] In order to compensate for the degradation of the height channel information, this embodiment corrects the ranging information. Specifically, the roll angle and pitch angle of the carrier obtained by IMU calculation are used to compensate the height of the ranging data to obtain the height of the carrier from the ground. The current height of the carrier is directly calculated using IMU and altimeter to avoid the failure of point cloud registration caused by the degradation of the height channel environment, thereby affecting the final positioning result.

[0073] The specific calculation is done using the following formula:

[0074]

[0075] in, Indicates the height of the carrier from the ground, l indicates the ranging information, Indicates the carrier roll angle calculated by IMU, Indicates the pitch angle of the carrier obtained by IMU calculation.

[0076] At this time, the carrier's six-degree-of-freedom posture can be divided into an external posture vector of the height channel and an internal posture vector of the horizontal channel. The external posture vector represents the vector that can be directly updated by the IMU and altimeter. The internal pose vector represents the x position, y position and heading angle that need to be updated by the IMU-assisted laser radar SLAM algorithm. In the pose estimation of the subsequent horizontal channel, this embodiment only needs to optimize and update the internal pose vector, which reduces the computational complexity.

[0077] S13, solving the point cloud data through the IMU to obtain the internal posture data of the carrier on the horizontal channel; the internal posture data includes the x position, y position and heading angle of the carrier;

[0078] In order to solve the horizontal channel information, this embodiment performs feature extraction, degradation estimation and ESKF tight coupling on the point cloud data, such as Figure 2 As shown, specifically:

[0079] S131, extracting geometric feature points of the point cloud data, performing point cloud registration on the geometric feature points, and obtaining a decoupled point cloud pose;

[0080] When extracting geometric feature points of point cloud data, this embodiment extracts geometric feature points by calculating and comparing the curvature information of each point for each point cloud data at each moment. The formula for calculating the curvature information is:

[0081]

[0082] Among them, c i The curvature of each point, P i is the target point, is a point cloud set within a certain range from the target point, P j is a point in the set of points near the target point;

[0083] In order to ensure uniform distribution of feature points, this embodiment divides the point cloud at a certain moment into six segments. Specifically, the point cloud of each segment is sorted according to the curvature. First, it is determined whether the current point is marked as a ground point. The N points with the largest curvature are selected from the non-ground points as edge points, and the N points with the smallest curvature are selected as plane points. For example, if the number of point clouds in this part is M, then N=1 / 13M, and the selection of N is adaptively adjusted according to the number of point clouds obtained by different beam numbers of multi-line laser radars.

[0084] In order to match the point cloud data, this embodiment uses point-to-line and point-to-plane feature matching methods according to the properties of edge points and plane points. Search in the point cloud at the previous moment and use the kd-tree method to search for a pair of matching points (p je ,p le ), establish the constraint of minimum distance from point to line:

[0085]

[0086] Among them, p je and p le are all matching points, p ie is the edge point, k is the current moment, k-1 is the previous moment; p k,ie is the current time point p ie The three-dimensional coordinates in the laser radar coordinate system, p k-1,je is the previous time point p je The three-dimensional coordinates in the laser radar coordinate system, p k-1,le is the previous time point p le The three-dimensional coordinates in the lidar coordinate system.

[0087] For the plane point at the current moment In the point cloud at the previous moment, the kd-tree method is used to search for the three nearest matching points that can form a triangle (p js ,p ls ,p ms ), establish the constraint of minimum distance from point to surface:

[0088]

[0089] Among them, p js 、p ls and p ms are all matching points, p is is a plane point, p k,is is the plane point p at the current moment is The three-dimensional coordinates in the laser radar coordinate system, p k-1,js is the matching point p at the previous moment jsThe three-dimensional coordinates in the laser radar coordinate system, p k-1,ls is the matching point p at the previous moment ls The three-dimensional coordinates in the laser radar coordinate system, p k-1,ms is the matching point p at the previous moment ms The three-dimensional coordinates in the lidar coordinate system.

[0090] Furthermore, the target equation is constructed through geometric feature points:

[0091]

[0092] Among them, T * To construct the objective equation, The laser point cloud pose of this embodiment is changed from six degrees of freedom in the traditional algorithm to three degrees of freedom through the previous IMU decoupling.

[0093] S132, performing degradation estimation on the decoupled point cloud pose to obtain a re-expressed pose estimation result;

[0094] For the optimization problem to be solved in pose estimation, it can be regarded as solving the overdetermined system of equations argmin x ‖Ax-b‖ 2 , and the optimal solution is recorded as x. In the degraded scenario, Figure 3 As shown in the figure, the constraint of minimum distance from point to line, the constraint of minimum distance from point to surface and the characteristic equation constitute constraint 1, constraint 2 and constraint 3 in the figure respectively. A perturbation constraint equation in the c direction is added to the original three constraint equations. The distance between the perturbation constraint equation and the optimal solution x of the original equation is d. After adding the perturbation constraint equation, the optimal solution is offset. At this time, the optimal solution obtained by the four constraint equations is x ′ =x+xd, where xd is the optimal solution offset.

[0095] Therefore, the degradation characteristic factor D is introduced to represent the degree of degradation of the laser radar in the three-degree-of-freedom posture direction after decoupling. Its expression is:

[0096]

[0097] In the lidar pose estimation problem, let the objective function T * The first-order Jacobian matrix of is the degradation detection matrix A, and the corresponding expression is

[0098]

[0099] The singular value decomposition method is used to obtain A T The three eigenvalues ​​of A [λ 1 ,λ 2 ,λ 3], and the three eigenvectors corresponding to the eigenvalues ​​[ε 1 ,ε 2 ,ε 3 ], which correspond to the three degrees of freedom after decoupling. According to the above derivation, the degradation characteristic factor D can be defined as λ i +1, (i = 1, 2, 3), and based on the three eigenvector directions, a degenerate feature space is formed. In this embodiment, the previous decoupled pose estimation result is re-expressed in the degenerate feature space for subsequent data fusion with the odometer.

[0100] In the degraded feature space, the covariance matrix E of the LiDAR pose estimation is a diagonal matrix, and the values ​​on its diagonal are proportional to the degraded feature factors. The corresponding expression is:

[0101]

[0102] Where k is a constant determined according to the current degraded environment and obtained through experiments in different degraded environments.

[0103] In this embodiment, the pose estimation covariance matrix in the degraded feature space is converted into the error covariance matrix in the lidar coordinate system through coordinate system transformation, and is used as observation noise for information fusion in the subsequent ESKF.

[0104] S133. According to the re-expressed pose estimation result, the error state quantity at the previous moment is updated through ESKF tight coupling to obtain the error state quantity at the current moment.

[0105] In order to obtain the position and posture data at the target time, this embodiment mainly includes two stages: prediction stage and update stage. On the basis of traditional Kalman filtering, error state module information is added, such as Figure 4 As shown, specifically: the carrier angular velocity and acceleration are obtained through the IMU, and the carrier angular velocity and acceleration are solved and integrated to obtain the main state of the system; the error state of the system is estimated through the IMU error covariance matrix, and the main state and the error state are superimposed to obtain the system state; according to the re-expressed posture estimation result, the error state and the system state are updated to obtain the error state quantity at the current moment. That is, in the update stage, this embodiment is based on the predicted state, and the error covariance matrix calculated by the degradation characteristic factor is used as the observation noise to update the error covariance matrix, and the error state and the system state are updated with the posture estimation result re-expressed by the laser radar in the degradation characteristic space to obtain a new round of state prediction value. Based on the traditional Kalman filter, this embodiment adds error state module information, and the state prediction value can be obtained more accurately through the prediction stage and the update stage.

[0106] S14. Perform posture estimation based on the external posture data and the internal posture data to obtain the posture data of the carrier.

[0107] In this embodiment, the external posture vector includes the height of the carrier from the ground, the roll angle and the pitch angle, and the internal posture data includes the x position, the y position and the heading angle. By separating the elevation channel from the horizontal channel and processing them in parallel, it not only pays attention to the degradation of the elevation channel, but also reduces the computational complexity of the horizontal channel degradation processing, ensuring the positioning accuracy while also ensuring the real-time performance of the algorithm. In order to further obtain high-precision positioning results, this embodiment obtains a high-frequency and high-precision laser radar and IMU data fusion odometer through continuous iterations of prediction and update to perform more accurate positioning.

[0108] Compared with the prior art, the present invention proposes a method for decoupling the carrier posture using IMU / altimeter, separates the elevation channel from the horizontal channel for parallel processing, pays attention to the degradation of the elevation channel, and reduces the computational complexity of the horizontal channel degradation processing, ensuring the positioning accuracy while also ensuring the real-time performance of the algorithm. Secondly, the present invention analyzes the current degradation information of the laser radar SLAM, and links the laser radar degradation characteristic factor with the error covariance matrix, improves the analysis ability of the degraded environment, is more targeted for the degraded environment, has a strong degradation processing ability, and can solve the laser radar SLAM failure problem in the degraded environment in a targeted manner. In addition, the present invention has made improvements to the sensor positioning system, and uses the Error-State Kalman filter (ESKF) method to tightly couple the IMU inertial navigation posture information and the laser radar time matching positioning information to obtain more accurate positioning information. And the system error state is processed, and the Jacobian matrix of the error state is calculated, and the computational complexity is low, which can meet the real-time requirements and facilitate the realization of a lightweight algorithm system.

[0109] Based on the above-mentioned method for simultaneous positioning and mapping by laser radar, an embodiment of the present invention further provides a system for simultaneous positioning and mapping by laser radar, such as Figure 5 As shown, the system comprises:

[0110] The altimeter 1 is used to obtain the ranging data of the carrier; the laser radar 2 is used to obtain the point cloud data of the carrier;

[0111] The IMU 3 includes an altitude channel solving module 31 and a horizontal channel solving module 32. The altitude channel solving module 31 is used to solve the ranging data through the IMU to obtain the external posture data of the carrier on the altitude channel; the external posture data includes the height of the carrier from the ground, the roll angle and the pitch angle. The horizontal channel solving module 32 is used to solve the point cloud data through the IMU to obtain the internal posture data of the carrier on the horizontal channel; the internal posture data includes the x position, y position and heading angle of the carrier;

[0112] The posture estimation module 4 is used to perform posture estimation based on the external posture data and the internal posture data to obtain the posture data of the carrier.

[0113] In another preferred embodiment, Figure 6 As shown, the height channel solution module 31 also includes a height compensation module 311, which is used to perform height compensation on the ranging data using the roll angle and pitch angle of the carrier obtained by IMU solution to obtain the height of the carrier from the ground.

[0114] In another preferred embodiment, Figure 7 As shown, the horizontal channel solving module 32 includes:

[0115] A feature extraction module 321 is used to extract geometric feature points of the point cloud data, perform point cloud registration on the geometric feature points, and obtain a decoupled point cloud pose;

[0116] A degradation estimation module 322, configured to perform degradation estimation on the decoupled point cloud pose to obtain a re-expressed pose estimation result;

[0117] The ESKF tight coupling module 323 is used to update the error state quantity at the previous moment through ESKF tight coupling according to the re-expressed pose estimation result to obtain the error state quantity at the current moment.

[0118] In another preferred embodiment, Figure 8 As shown, the IMU 3 also includes a motion compensation module 33 for performing motion compensation on the point cloud data.

[0119] For the specific definition of a system for simultaneous positioning and mapping by a laser radar, please refer to the definition of a method for simultaneous positioning and mapping by a laser radar above, which will not be repeated here. Each module in the above system can be implemented in whole or in part by software, hardware, and a combination thereof. The above modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0120] In summary, the present invention provides a method and system for simultaneous positioning and mapping of a laser radar, the method comprising: obtaining ranging data of a carrier through an altimeter, and obtaining point cloud data of the carrier through a laser radar; solving the ranging data through an IMU to obtain external posture data of the carrier on a height channel; the external posture data includes the height of the carrier from the ground, the roll angle, and the pitch angle; solving the point cloud data through an IMU to obtain internal posture data of the carrier on a horizontal channel; the internal posture data includes the x position, y position, and heading angle of the carrier; posture estimation is performed based on the external posture data and the internal posture data to obtain the posture data of the carrier. The present invention can solve the problem of reduced positioning accuracy of laser radar in a degraded environment.

[0121] Each embodiment in this specification is described in a progressive manner, and the same or similar parts of each embodiment can be directly referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment. It should be noted that the technical features of the above-mentioned embodiments can be combined arbitrarily. In order to make the description concise, all possible combinations of the technical features in the above-mentioned embodiments are not described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0122] The above-mentioned embodiments only express several preferred implementation modes of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for ordinary technicians in the technical field, several improvements and substitutions can be made without departing from the technical principles of the present invention, and these improvements and substitutions should also be regarded as the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be based on the protection scope of the claims.

Claims

1. A method for simultaneous positioning and mapping using a laser radar, characterized in that: The method comprises: Acquiring ranging data of the carrier by means of an altimeter, and acquiring point cloud data of the carrier by means of a laser radar; The distance measurement data is solved by the IMU to obtain the external posture data of the carrier on the height channel; the external posture data includes the height of the carrier from the ground, the roll angle and the pitch angle; The point cloud data is solved by the IMU to obtain the internal posture data of the carrier on the horizontal channel, specifically including: Extracting geometric feature points of the point cloud data, performing point cloud registration on the geometric feature points, and obtaining a decoupled point cloud pose; Performing degradation estimation on the decoupled point cloud pose to obtain a re-expressed pose estimation result; According to the re-expressed pose estimation result, the error state quantity at the previous moment is updated through ESKF tight coupling to obtain the error state quantity at the current moment; the internal pose data includes the x position, y position and heading angle of the carrier; The performing point cloud registration on the geometric feature points comprises: For the edge points in the geometric feature points, a pair of matching points with the shortest distance and not on the same laser beam are searched by the kd-tree method, and a constraint of minimum distance from point to line is established; For the plane points in the geometric feature points, the kd-tree method is used to search for three matching points that are closest to each other and can form a triangle, and a constraint of minimum distance from point to plane is established; Constructing a target equation according to the constraint of the minimum distance from the point to the line and the constraint of the minimum distance from the point to the surface; The edge point is a point with the largest curvature among non-ground points, and the plane point is a point with the smallest curvature among non-ground points; Perform posture estimation based on the external posture data and the internal posture data to obtain the posture data of the carrier.

2. The method for simultaneous laser radar positioning and mapping according to claim 1, characterized in that: The step of solving the ranging data by the IMU to obtain the external posture data of the carrier on the height channel includes: The rolling angle and pitch angle of the carrier obtained by IMU calculation are used to perform height compensation on the ranging data to obtain the height of the carrier from the ground.

3. The method for simultaneous laser radar positioning and mapping according to claim 2, characterized in that: The roll angle and pitch angle of the carrier obtained by IMU solution are used to perform height compensation on the ranging data to obtain the height of the carrier from the ground. The following formula is used for correction calculation: in, Indicates the height of the carrier from the ground, l indicates the ranging information, Indicates the carrier roll angle calculated by IMU, Indicates the pitch angle of the carrier obtained by IMU solution.

4. The method for simultaneous laser radar positioning and mapping according to claim 1, characterized in that: The step of updating the error state quantity at the previous moment by tight coupling with the ESKF according to the re-expressed pose estimation result to obtain the error state quantity at the current moment includes: The angular velocity and acceleration of the carrier are obtained through the IMU, and the angular velocity and acceleration of the carrier are solved and integrated to obtain the main state of the system; The error state of the system is estimated by using the IMU error covariance matrix, and the main state is superimposed on the error state to obtain the system state; According to the re-expressed pose estimation result, the error state and the system state are updated to obtain the error state quantity at the current moment.

5. The method for simultaneous laser radar positioning and mapping according to claim 1, characterized in that: The constraint of establishing the minimum distance from point to line is specifically based on the following formula: Among them, p je and p le are all matching points, p ie is the edge point, k is the current moment, k-1 is the previous moment; p k,ie is the current time point p ie The three-dimensional coordinates in the laser radar coordinate system, p k-1,je is the previous time point p je The three-dimensional coordinates in the laser radar coordinate system, p k-1,le is the previous time point p le Three-dimensional coordinates in the laser radar coordinate system; The constraint of establishing the minimum distance from point to surface is specifically established by the following formula: Among them, p js 、p ls and p ms are all matching points, p is is a plane point, p k,is is the plane point p at the current moment is The three-dimensional coordinates in the laser radar coordinate system, p k-1,js is the matching point p at the previous moment js The three-dimensional coordinates in the laser radar coordinate system, p k-1,ls is the matching point p at the previous moment ls The three-dimensional coordinates in the laser radar coordinate system, p k-1,ms is the matching point p at the previous moment ms Three-dimensional coordinates in the laser radar coordinate system; The target equation is constructed by using the following formula: Among them, T * To construct the objective equation, is the pose change between the current moment and the previous moment.

6. The method for simultaneous laser radar positioning and mapping according to claim 1, characterized in that: The step of obtaining point cloud data of the carrier by using a laser radar further includes: The point cloud data is motion compensated by the IMU.

7. The method for simultaneous laser radar positioning and mapping according to claim 6, characterized in that: The performing motion compensation on the point cloud data by using the IMU includes: Calculate the pose of the IMU data at the current moment and the previous moment, and use the linear interpolation method to calculate the pose in the IMU coordinate system at any moment.

8. A laser radar simultaneous positioning and mapping system, characterized in that: The system includes: an altimeter, a laser radar, an IMU and a posture estimation module, wherein: The altimeter is used to obtain the ranging data of the carrier; the laser radar is used to obtain the point cloud data of the carrier; The IMU includes a height channel solving module and a horizontal channel solving module, wherein the height channel solving module is used to solve the ranging data through the IMU to obtain the external posture data of the carrier on the height channel; the external posture data includes the height of the carrier from the ground, the roll angle and the pitch angle; the horizontal channel solving module is used to solve the point cloud data through the IMU to obtain the internal posture data of the carrier on the horizontal channel; the internal posture data includes the x position, y position and heading angle of the carrier; the horizontal channel solving module also includes: A feature extraction module is used to extract geometric feature points of the point cloud data, perform point cloud registration on the geometric feature points, and obtain a decoupled point cloud pose; A degradation estimation module, used for performing degradation estimation on the decoupled point cloud pose to obtain a re-expressed pose estimation result; An ESKF tight coupling module is used to update the error state quantity at the previous moment through ESKF tight coupling according to the re-expressed pose estimation result to obtain the error state quantity at the current moment; The performing point cloud registration on the geometric feature points comprises: For the edge points in the geometric feature points, a pair of matching points with the shortest distance and not on the same laser beam are searched by the kd-tree method, and a constraint of minimum distance from point to line is established; For the plane points in the geometric feature points, the kd-tree method is used to search for three matching points that are closest to each other and can form a triangle, and a constraint of minimum distance from point to plane is established; Constructing a target equation according to the constraint of the minimum distance from the point to the line and the constraint of the minimum distance from the point to the surface; The edge point is a point with the largest curvature among non-ground points, and the plane point is a point with the smallest curvature among non-ground points; The posture estimation module is used to perform posture estimation based on the external posture data and the internal posture data to obtain the posture data of the carrier.

9. The laser radar simultaneous positioning and mapping system according to claim 8, characterized in that: The height channel solving module also includes a height compensation module for The rolling angle and pitch angle of the carrier obtained by IMU calculation are used to perform height compensation on the ranging data to obtain the height of the carrier from the ground.

10. The laser radar simultaneous positioning and mapping system according to claim 8, characterized in that: The IMU also includes a motion compensation module for performing motion compensation on the point cloud data.

Images