A laser odometry method based on sliding window grid local map
By using a sliding window grid local map method, the problems of insufficient accuracy or excessive resource consumption of existing laser odometers are solved, and a high-efficiency laser odometer is realized under limited hardware conditions.
Patent Information
- Application Number
- CN202310413713.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-04-18
- Publication Date
- 2025-11-07
- Estimated Expiration
- 2043-04-18
AI Technical Summary
Existing laser odometry methods are simple to calculate but lack accuracy, or they have sufficient accuracy but require high computing power and memory.
The laser odometry method using a sliding window grid local map ensures calculation accuracy while requiring minimal computing power and memory.
It realizes a high-efficiency laser odometry under limited hardware conditions, which ensures accuracy while reducing the consumption of computing resources, making it suitable for mobile platforms.
Smart Images
Figure CN116679312B_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The application relates to the technical field of laser odometry, in particular to a laser odometry method based on a sliding window grid local map. BACKGROUND
[0002] In recent years, with the development of laser radar technology, the upper limit of the performance of laser radar is higher and higher, and the cost is lower and lower, and laser radar has been widely applied to the fields of robots, unmanned aerial vehicles, unmanned driving, security monitoring, surveying and mapping and the like, and is one of the most important sensors for realizing the positioning, sensing and environment model reconstruction of intelligent carriers such as robots. Among them, the laser-based odometry calculation is a very important part in many applications, and is similar to the wheel odometry, which obtains the motion trajectory and the latest pose of the laser radar through processing and iterative pose calculation on the data collected in the continuous motion process, and the difference is that the wheel odometry is based on the rolling calculation of the encoder on the wheel, and the laser odometry is based on the calculation of each frame of laser point cloud data, so it does not have the disadvantage of being affected by the unevenness or slip of the ground as the wheel odometry.
[0003] The laser odometry has very important significance in the laser-based map construction and laser-based positioning. This paper only discusses the laser odometry in two-dimensional space. The existing laser odometry methods are based on the registration of single-frame laser point clouds, such as icp, ndt and csm registration methods for iterative registration calculation of continuous single-frame laser point clouds, or based on the registration of single-frame laser point clouds and local maps, such as the cartography system open sourced by Google. These methods either have insufficient accuracy or require high computing power and large memory. SUMMARY
[0004] The application solves the problems that the laser odometry either has simple calculation but insufficient accuracy or has sufficient accuracy but high computing power and memory occupation, and proposes a laser odometry method based on a sliding window grid local map, which calculates the laser odometry based on the sliding window grid local map, thereby guaranteeing the accuracy of the laser odometry calculation, and requiring small computing power and memory, and being an effective method which takes into account the performance and resource consumption.
[0005] To achieve the above object, the following technical scheme is proposed.
[0006] A laser odometry method based on a sliding window grid local map comprises the following steps:
[0007] S1, setting an initial local map with fixed memory, and setting the coordinate range of the initial local map, and performing griding on the initial local map with a certain size;
[0008] s2, acquire the first frame of laser point cloud data of the laser odometry, and update the grid value of the local map according to the laser position;
[0009] S3, starting from the second frame of laser point cloud data, each frame of laser point cloud data is first registered with the local map, and then the grid value of the local map is updated, and if the laser scanning exceeds the initial local map coordinate range, the grid is updated in a loop to form a sliding window grid map that moves with the laser.
[0010] Since the sliding window local map moves with the laser in the present application, the reference coordinate is always established by the first frame of laser data, so the registration result of each laser frame and the local map is the relative motion with the first frame of laser data, rather than the relative motion with the last frame, so the registration result is directly the odometry coordinate of the latest laser frame. The present application not only guarantees the accuracy advantage of the frame and local map registration method, but also has constant level stable memory occupation and constant level stable computing resource occupation, and because most of the iteration process only needs to look up the table, the computing efficiency is also very high, which is very suitable for mobile platforms with limited hardware conditions.
[0011] As a preferred, the process of registration with the local map is as follows:
[0012] S201, set the optimization function as:
[0013]
[0014] Where n is the number of the nearest point pairs, Pi is a point in the target laser point cloud P, Qi is the nearest point in the reference laser point cloud Q corresponding to Pi, ΔR is the rotation matrix, Δt is the translation vector, and ΔR and Δt are the rotation and translation of the target point cloud relative to the reference laser point cloud.
[0015] S202, based on the gradient descent method, solve the optimization function to obtain ΔR and Δt to minimize E(ΔR, Δt).
[0016] As a preferred, the S202 specifically includes the following steps:
[0017] S221, for each point in the target laser point cloud, based on the current ΔR, Δt transformation, match the nearest point in the reference laser point cloud;
[0018] S222, obtain E(ΔR, Δt) from the optimization function, in order to reduce E(ΔR, Δt), solve the gradient of E(ΔR, Δt) with respect to ΔR and Δt;
[0019] S223, iterate ΔR and Δt in the direction of gradient descent, update the current ΔR and Δt;
[0020] S224, loop S221 to S223 until the matching degree E(AR, At) between the two point clouds is less than a threshold value.
[0021] As a preference, the grid values include dx, dy and dis, the dx is the difference of x coordinates between the corresponding points of the point clouds P and Q at each iteration, the dy is the difference of y coordinates between the corresponding points of the point clouds P and Q at each iteration, and the dis is the Euclidean distance between the corresponding points.
[0022] As a preference, the solving process of the dx, dy and dis is as follows:
[0023] For two-dimensional laser point clouds, there are:
[0024]
[0025] is the rotation and translation transform of the target laser point cloud coordinate system relative to the reference laser point cloud coordinate system, wherein Δx, Δy and Δθ are the relative position and angle,
[0026] Let Then:
[0027]
[0028]
[0029] dx = cosΔθ*Pxi - sinΔθ*Pyi + Δx - Qxi;
[0030] dy = sinΔθ*Pxi + cosΔθ*Pyi + Δy - Qyi;
[0031]
[0032] Let E(AR, At) be the partial derivative of Δx, Δy and Δθ
[0033]
[0034]
[0035]
[0036]
[0037] As a preference, the process of updating the grid values of the local map is as follows: when inserting new 1-frame laser point cloud data, for each point in the new 1-frame, judge the surrounding grid, if the dis stored in the original grid is larger than the distance between the point and the new laser point, the value in the grid needs to be updated with the new laser point, otherwise the value in the grid does not need to be updated.
[0038] The beneficial effects of the present application are: because the local map of the sliding window is moved with the laser, the reference coordinates are always established by the first frame of laser data, so the registration result of each laser frame and the local map is the relative motion with the first frame of laser data, not the relative motion with the last frame, so the registration result is directly the odometer coordinates of the latest laser frame. The present application not only guarantees the accuracy advantage of the frame and local map registration method, but also has constant level stable memory occupation, constant level stable computing resource occupation, and because most of the iteration process only needs to look up the table, the computing efficiency is also very high, which is very suitable for mobile platforms with limited hardware conditions. BRIEF DESCRIPTION OF DRAWINGS
[0039] Figure 1 is a schematic diagram of a cyclic sliding window grid. DETAILED DESCRIPTION
[0040] Embodiment:
[0041] The present embodiment proposes a laser odometer method based on a sliding window grid local map, and the derivation process of the method is as follows:
[0042] Laser radar refers to a radar system that uses laser to detect and measure distance. The present application studies a two-dimensional laser radar, which rotates a laser detector on a plane to scan and can obtain ranging information of the environment on the scanning plane. The rotation angle of the laser detector is obtained through an angle sensor such as an encoder of the radar itself, so that the two-dimensional laser radar can obtain ranging and angle information of the environmental features on the scanning plane when working.
[0043] The two-dimensional space coordinates are represented by (x, y, θ), wherein θ represents the heading angle, and when represented by a rotation matrix R and a translation matrix t,
[0044] Similar to the wheeled odometer, the laser odometer is also based on continuous sensor data to iteratively calculate the mileage. For example, if the coordinates of the laser at the i-th frame of point cloud data are (R i , t i ), the relative motion (ΔR, Δt) of the i+1-th frame of point cloud data and the i-th frame of point cloud data is calculated, then the coordinates of the laser at the i+1-th frame of point cloud data are (R i+1 , t i+1 ), wherein R i+1 =R i *ΔR, t i+1 =t i +R i *Δt. By continuously iterating the motion of each frame of laser point cloud data relative to the last frame and superimposing the calculation, the coordinates of the laser corresponding to each frame of laser point cloud data can be obtained. This is the basic calculation process of the laser odometer.
[0045] The calculation of the relative motion of two adjacent frames of laser odometry has developed from frame-to-frame registration to frame-to-local map registration, and frame-to-local map registration is now a common practice. The local map refers to the superposition of multiple frames of spatially continuous laser point cloud data to form a local small map. Frame-to-local map registration means that each frame of laser point cloud data is not registered with the adjacent previous frame of laser point cloud data to calculate the relative motion, but is registered with the local map containing the previous frame to calculate the relative motion, which can significantly reduce error accumulation and improve the accuracy of laser odometry. This paper adopts frame-to-local map registration, and the difference lies in that the following points are considered when designing the method: 1) the memory consumption is constant and will not increase with the expansion of the map; 2) the operation amount is constant and will not increase with the increase of the number of frames in the local map; 3) the calculation efficiency is high enough to be used on general low-cost mobile robots.
[0046] The present application is based on the most classic ICP expansion design: ICP algorithm is a registration method based on gradient descent method, which iteratively selects corresponding point pairs, calculates the optimal relative motion transformation, and stops until the registration convergence accuracy requirement is met. The purpose of ICP is to find the rotation ΔR and translation Δt, so that the laser point clouds in two different coordinate systems completely overlap after the target laser point cloud is rotated and translated in the reference laser point cloud coordinate system.
[0047] Its optimization function is:
[0048]
[0049] Where n is the number of nearest point pairs, Pi is a point in the target laser point cloud P, Qi is the nearest point in the reference laser point cloud Q corresponding to Pi, ΔR is the rotation matrix, Δt is the translation vector, and ΔR and Δt are the rotation and translation of the target point cloud relative to the reference laser point cloud. The purpose of optimization is to find ΔR and Δt to minimize E(ΔR, Δt).
[0050] The steps based on gradient descent method are as follows:
[0051] (1) For each point in the target laser point cloud, match the nearest point in the reference laser point cloud based on the current ΔR, Δt transformation
[0052] (2) Obtain E(ΔR, Δt) from the optimization function, in order to reduce E(ΔR, Δt), solve the gradient of E(ΔR, Δt) with respect to ΔR and Δt
[0053] (3) iteratively update the current ΔR and Δt along the gradient descent direction
[0054] (4) cycle (1) (2) (3) until the matching degree E (AR, At) between two point clouds is less than a threshold.
[0055] For two-dimensional laser point clouds, is the rotation translation transform of the target laser point cloud coordinate system to be solved relative to the reference laser point cloud coordinate system, where Ax, Ay, A0 is the relative position and angle,
[0056] Let Then:
[0057]
[0058]
[0059] dx = cos A0 * Pxi - sin A0 * Pyi + Ax - Qxi;
[0060] dy = sin A0 * Pxi + cos A0 * Pyi + Ay - Qyi;
[0061]
[0062] Let E (AR, At) be the partial derivative of Ax, Ay, A0
[0063]
[0064]
[0065]
[0066] where dx is the difference between the x coordinates of the corresponding points of the point clouds P and Q at each iteration, dy is the difference between the y coordinates of the corresponding points of the point clouds P and Q at each iteration, is the Euclidean distance between the corresponding points, Ax, Ay, A0 is the current laser point cloud P coordinate system relative to the laser point cloud Q coordinate system at each iteration. From the above derivation, at each iteration:
[0067] The partial derivative of the optimization objective with respect to Ax is only related to the Euclidean distance and x deviation between the corresponding points of the two point clouds;
[0068] The partial derivative of the optimization objective with respect to Ay is only related to the Euclidean distance and y deviation between the corresponding points of the two point clouds;
[0069] The partial derivative of the optimization objective with respect to A0 is related to the Euclidean distance, x deviation, y deviation, current A0, x, y coordinates of the target point cloud between the corresponding points of the two point clouds.
[0070] Because the reference point cloud is always unchanged in the iteration process, the reference point cloud coordinate system is rasterized, so for a certain grid, no matter which point in the target point cloud falls into it, the values of dx, dy, and dis of the corresponding point Qi are fixed, and the partial derivative of E (AR, At) with respect to Ax, Ay, and A0 is mainly related to these values. Therefore, the reference point cloud coordinate system is rasterized, and dx, dy, and dis of the nearest Qi point of each grid are calculated in advance and stored. In the iteration process, each iteration only needs to calculate which grid each point of the target point cloud falls into, and the calculation value of the partial derivative can be directly queried without searching for the corresponding Qi point of Pi each time. This can significantly reduce the calculation amount in the iteration process. Especially when the reference laser point cloud data is very large, such as the reference laser point cloud being the entire scene map or a local map, the point cloud scale is very large. If the rasterization is done in advance and the nearest neighbor dx, dy, and dis stored in each grid are calculated, the calculation amount of the target laser point cloud coordinate system is constant and very fast.
[0071] In the calculation process of the laser odometry, the laser point cloud data comes one frame after another. If the size of the map is known, rasterization is done in advance, and when each frame of laser point cloud data comes, a constant and stable algorithm is selected to update the values of dx, dy, and dis in the grid, and the calculation amount of this constant algorithm is acceptable. Therefore, the entire laser odometry algorithm is of constant operation amount, and even if the number of laser point cloud frames in the local map increases, the operation amount is still constant and stable.
[0072] Therefore, when each frame of laser data comes, the registration is divided into two steps: the first step is to register the current laser frame with the local map; the second step is to update the grid values of the local map with the registered laser frame.
[0073] The first step has been demonstrated above to be of constant level, and because most of the calculations only need to look up the table, the calculation efficiency is very high.
[0074] The second step of updating the local map algorithm, because each grid stores the distance and x, y deviation of the nearest point in the grid, this feature provides the feasibility of dynamically updating the grid values. When inserting a new laser frame, only the surrounding grid of each point in the new laser frame needs to be judged. If the dis stored in the original grid is larger than the distance of the new laser point, the value of the grid needs to be updated with the new laser point, otherwise the value in the grid does not need to be updated.
[0075] Therefore, using these two steps of algorithm, the local map and the new laser frame can be quickly and stably updated.
[0076] In the foregoing description of updating the grid local map, it is assumed that the map coordinate scale is known so as to rasterize in advance and update the grid. The map scale can be set very large, and this scheme is indeed feasible, but it will consume a large amount of memory, and therefore the memory problem needs to be solved.
[0077] Suppose that the farthest distance that can be detected by the laser is 50 meters, and the initial coordinates of the robot are (0, 0), then the xy boundaries of the initial grid local map are set to (-100, 100) meters, and the initial laser frame can be contained in the grid local map, but when the laser moves, the laser frame will scan to the boundary of the initial grid map, and even to the outside, and the algorithm will fail. However, because the laser is continuously moving, the laser will only slowly scan to the outside of the boundary, and the other end of the boundary will be far away from the laser data, and the registration of the current frame laser will not be affected, and therefore when the laser detects one end of the grid map, the data at the other end can be covered. Thus, a sliding window grid local map structure can be designed. Taking a one-dimensional array as an example, as shown in the following table, the data in the initial grid map is stored in 0-6, if the data is extended to the right, the data will cover the left area of the map, and if the data is extended to the left, the new data will cover the right area of the map. Figure 1 A fixed-scale local map can be updated to coordinate data near the laser coordinates as the laser moves. Thus, a fixed memory size grid map can be used to maintain a latest grid map. Through the foregoing description, it is known that the grid local map can be maintained and registered with constant level of operation amount by using fixed size memory. Figure 1
[0078] The laser odometry method based on the sliding window grid local map of the embodiment includes the following steps.
[0079] S1, set an initial local map of fixed memory, set the coordinate range of the initial local map, and rasterize the initial local map with a certain size; first apply a block of memory, the coordinate range of the initial local map is x (-100, 100) meters, y (-100, 100) meters, and the size of each grid is 2 cm after rasterization;
[0080] S2, acquire the first frame of laser point cloud data of the laser odometry, and update the grid value of the local map according to the laser position; the process of updating the grid value of the local map is as follows: when inserting the new first frame of laser point cloud data, the grid around each point in the new first frame is judged, if the dis stored in the original grid is larger than the distance between the point and the new laser point, the value in the grid needs to be updated by the new laser point, otherwise the value in the grid does not need to be updated.
[0081] S3, starting from the second frame of laser point cloud data, for each frame of laser point cloud data, first, registration is made with the local map, and then the grid values of the local map are updated, the grid values including dx, dy and dis, the dx being the difference between the x coordinates of the corresponding points of the point clouds P and Q at each iteration, the dy being the difference between the y coordinates of the corresponding points of the point clouds P and Q at each iteration, and the dis being the Euclidean distance between the corresponding points. If the laser scanning exceeds the initial local map coordinate range, the grid is updated in a loop to form a sliding window grid map that moves with the laser.
[0082] The registration with the local map is as follows:
[0083] S201, the optimization function is set as:
[0084]
[0085] wherein n is the number of the nearest point pairs, Pi is a point in the target laser point cloud P, Qi is the nearest point in the reference laser point cloud Q corresponding to Pi, ΔR is a rotation matrix, Δt is a translation vector, and ΔR and Δt are the rotation and translation of the target point cloud relative to the reference laser point cloud;
[0086] S202, the optimization function is solved based on the gradient descent method to obtain ΔR and Δt that minimize E(ΔR, Δt).
[0087] The S202 specifically includes the following steps:
[0088] S221, for each point in the target laser point cloud, the nearest point in the reference laser point cloud is matched based on the current ΔR and Δt transformation;
[0089] S222, E(ΔR, Δt) is obtained from the optimization function, and the gradient of E(ΔR, Δt) with respect to ΔR and Δt is solved in order to reduce E(ΔR, Δt).
[0090] S223, ΔR and Δt are iterated in the direction of the gradient descent, and the current ΔR and Δt are updated;
[0091] S224, S221 to S223 are looped until the matching degree E(ΔR, Δt) between the two point clouds is less than a threshold value.
[0092] The solving process of the dx, dy and dis is as follows:
[0093] For a two-dimensional laser point cloud, there are:
[0094]
[0095] is the rotation and translation transformation of the target laser point cloud coordinate system to be solved relative to the reference laser point cloud coordinate system, wherein Δx, Δy and Δθ are the relative position and angle.
[0096] Let Then we have:
[0097]
[0098]
[0099] dx = cos Δθ * Pxi - sin Δθ * Pyi + Δx - Qxi;
[0100] dy = sin Δθ * Pxi + cos Δθ * Pyi + Δy - Qyi;
[0101]
[0102] Let E(ΔR, Δt) be partial derivative of Δx, Δy, Δθ
[0103]
[0104]
[0105]
[0106]
[0107] Because our sliding window local map is moving with the laser, the reference coordinate is always the first frame of laser data, so the registration result of each laser frame and local map is the relative motion with the first frame of laser data, not the relative motion with the last frame, so the registration result is directly the odometer coordinate of the latest laser frame.
[0108] Through the above design, our laser odometer method not only guarantees the accuracy advantage of the frame and local map registration method, but also has constant level stable memory occupation, constant level stable computing resource occupation, and because the iteration process only needs to look up the table most of the time, its computing efficiency is also very high, and it is very suitable for mobile platforms with limited hardware conditions.
Claims
1. A laser odometry method based on sliding window grid local map, characterized in that, The method comprises the following steps: S1, setting an initial local map of fixed memory, setting a coordinate range of the initial local map, and rasterizing the initial local map in a certain size; S2, acquiring first frame laser point cloud data of a laser odometer, and updating a grid value of the local map according to a laser position; S3, starting from second frame laser point cloud data, each frame laser point cloud data is firstly registered with the local map, and then the grid value of the local map is updated, and if laser scanning exceeds the coordinate range of the initial local map, the grid is cyclically updated to form a sliding window grid map moving with the laser.
2. The laser odometry method based on sliding window grid local map according to claim 1, characterized in that, The registration process with the local map is as follows: S201, setting an optimization function as: Wherein n is the number of the nearest point pairs, Pi is a point in the target laser point cloud P, Qi is the nearest point corresponding to Pi in the reference laser point cloud Q, ΔR is a rotation matrix, Δt is a translation vector, and ΔR and Δt are the rotation and translation of the target point cloud relative to the reference laser point cloud; S202, solving the optimization function based on the gradient descent method to obtain ΔR and Δt to minimize E(ΔR, Δt).
3. The laser odometry method based on sliding window grid local map according to claim 2, characterized in that, The S202 specifically comprises the following steps: S221, for each point in the target laser point cloud, the nearest point in the reference laser point cloud is matched based on the current ΔR and Δt transformation; S222, E(ΔR, Δt) is obtained from the optimization function, and the gradient of E(ΔR, Δt) with respect to ΔR and Δt is solved in order to reduce E(ΔR, Δt); S223, ΔR and Δt are iterated in the gradient descent direction to update the current ΔR and Δt; S224, S221 to S223 are cycled until the matching degree E(ΔR, Δt) between the two point clouds is less than a threshold value.
4. The laser odometry method based on sliding window grid local map according to claim 3, characterized in that, The grid value comprises dx, dy and dis, the dx is the difference between the x coordinates of the corresponding points of the point clouds P and Q at each iteration, the dy is the difference between the y coordinates of the corresponding points of the point clouds P and Q at each iteration, and the dis is the Euclidean distance between the corresponding points.
5. The method of claim 4, wherein, The solving process of the dx, dy and dis is as follows: For two-dimensional laser point cloud, there are: It is a rotation and translation transformation of the target laser point cloud coordinate system to be solved relative to the reference laser point cloud coordinate system, wherein Δx, Δy and Δθ are the relative position and angle, Let Then, dx = cosΔθ*Pxi-sinΔθ*Pyi+Δx-Qxi; dy = sinΔθ*Pxi+cosΔθ*Pyi+Δy-Qyi; Let E(ΔR, Δt) be partial derivative of Δx, Δy and Δθ 6. The method of claim 4, wherein, The process of updating the grid value of the local map is as follows: when a new 1 frame of laser point cloud data is inserted, for each point in the new 1 frame, the surrounding grid is judged, if the dis stored in the original grid is larger than the distance between the point and the new laser point, the value in the grid needs to be updated with the new laser point, otherwise the value in the grid does not need to be updated.