Two-dimensional direct laser odometry calculation method and system applied to unmanned vehicles

Abstract

The application discloses a two-dimensional direct laser odometer calculation method and system applied to unmanned driving. Firstly, high-frequency sector scanning original data of a two-dimensional laser radar is received; the original data is converted and removed to obtain sector point set data; secondly, based on a static assumption, an initial map and a search KD-Tree are constructed; the sector scanning original data is continuously received, a radar coordinate system positioning score matrix and a rotating positioning point set matrix are constructed; the positioning score matrix and the rotating positioning point set matrix are updated; then, according to the positioning score matrix and the rotating positioning point set matrix, an incremental displacement and an incremental angular displacement are estimated; finally, based on a kinematic model, the incremental displacement is estimated to a global displacement through a low-pass filter and a Kalman filter, odometer data is obtained; and according to a global pose estimation result, the map and the KD-Tree are updated. The application avoids a matching process, reduces the demand for map features, and thus can estimate the mileage when the environment features are few.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of laser radar, and relates to a two-dimensional direct laser odometer calculation method and system applied to unmanned driving. BACKGROUND

[0002] In the field of mobile robots, odometer is a key technology for robots to estimate their own motion, and the traditional wheeled odometer is easily affected by wheel slip and uneven ground, thereby generating errors and accumulating. In order to improve the accuracy, direct laser odometer emerges as the times require. Direct laser odometer directly calculates the pose change by using laser radar data, without relying on other external positioning systems, and has good adaptability in complex environments.

[0003] Generally speaking, direct laser radar odometer is a method for determining the position of a robot by analyzing consecutive laser radar scan frames. This involves the application of scan frame matching, and according to the scan matching method, direct laser odometer can be divided into two types: (1) direct matching; (2) feature-based matching.

[0004] For the direct matching method, it is mainly realized by ICP matching (Iterative Closest Point), and its core idea is to find the optimal rigid body transformation (rotation and translation) by iterative optimization, so as to minimize the distance between two point clouds. For the feature-based matching method, it is mainly PL-ICP (Point-to-Line Iterative Closest Point), which introduces point-to-line matching compared with the traditional ICP method, thereby improving the matching accuracy and robustness. However, the above schemes both need map features and iterative calculation, and have low environmental adaptability and poor real-time performance.

[0005] In recent years, with the deepening of research, direct laser odometer independent of point matching method has also developed. RF2O (Range Flow-based 2D Odometry) is a 2d laser radar odometer method based on distance flow estimation. This scheme estimates the motion of the robot by analyzing the "distance flow" between consecutive frames of laser radar, avoids the complexity of feature extraction in traditional methods, and has good real-time performance and adaptability. However, RF2O is prone to calculation drift when there are few objects in the environment that can be used for flow estimation. SUMMARY

[0006] For the existing laser odometer, the pose calculation is often matching and estimating the single-frame radar point cloud message published by the radar SDK, and the update frequency can only be 6-12Hz, and when the radar intra-frame distortion is introduced due to high-speed motion, the positioning accuracy will be greatly reduced, and even the positioning operation cannot be completed; at the same time, due to the low message frequency of the radar frame, the delay of the laser odometer system is high, and the bandwidth is limited, which is not conducive to the system control realized based on the odometer.

[0007] To solve the above problems, the present application provides a two-dimensional direct laser odometer calculation method and system applied to unmanned driving.

[0008] In the first aspect, the present application provides a two-dimensional direct laser odometer calculation method applied to unmanned driving, comprising the following steps:

[0009] Data receiving and preprocessing:

[0010] Receiving high-frequency sector scanning original data of two-dimensional laser radar;

[0011] Converting and rejecting the original data to obtain standardized sector point set data;

[0012] Initial map construction:

[0013] Based on the static assumption, an initial map and a search KD-Tree are constructed;

[0014] Dynamic search and positioning score matrix update:

[0015] Continuously receiving sector scanning original data, constructing a radar coordinate system positioning score matrix and a rotating positioning point set matrix;

[0016] According to the reverse motion assumption, the positioning score matrix and the rotating positioning point set matrix are updated;

[0017] Incremental pose estimation:

[0018] According to the positioning score matrix and the rotating positioning point set matrix, the incremental displacement and the incremental angular displacement are estimated;

[0019] Global pose estimation:

[0020] Based on the kinematic model, the incremental displacement is estimated to the global displacement through a low-pass filter and a Kalman filter to obtain the odometer data;

[0021] Map update:

[0022] According to the global pose estimation result, the map and the KD-Tree are updated.

[0023] In a second aspect, the present application provides a two-dimensional direct laser odometry calculation system applied to unmanned vehicles, comprising:

[0024] a data receiving and preprocessing module, configured to:

[0025] receive high-frequency sector scanning raw data of a two-dimensional laser radar;

[0026] convert and eliminate the raw data to obtain standardized sector point set data;

[0027] an initial map construction module, configured to:

[0028] construct an initial map and a KD-Tree based on a static assumption;

[0029] a dynamic search and positioning score matrix updating module, configured to:

[0030] continuously receive sector scanning raw data, construct a radar coordinate system positioning score matrix and a rotating positioning point set matrix;

[0031] update the positioning score matrix and the rotating positioning point set matrix according to a reverse motion assumption;

[0032] an incremental pose estimation module, configured to:

[0033] estimate an incremental displacement and an incremental angular displacement according to the positioning score matrix and the rotating positioning point set matrix;

[0034] a global pose estimation module, configured to:

[0035] estimate a global displacement by a low-pass filter and a Kalman filter based on a kinematic model, to obtain odometry data;

[0036] a map updating module, configured to:

[0037] update a map and a KD-Tree according to a global pose estimation result.

[0038] In a third aspect, the present application provides an electronic device, comprising:

[0039] a processor;

[0040] a memory for storing processor executable instructions;

[0041] wherein the processor implements the above method by running the executable instructions

[0042] In a fourth aspect, the present application provides a computer readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the above method.

[0043] The beneficial effects of this invention are as follows: The odometer calculation of this invention utilizes high-frequency raw laser data with the same update frequency, overcoming the disadvantage of low frequency in other odometer schemes. Simultaneously, the pose incremental estimation method based on relative motion and the scoring positioning matrix avoids the extraction of environmental features, thus reducing dependence on environmental features. Furthermore, addressing the issue of incremental rather than global motion estimation, the system is modeled, and then a low-pass + extended Kalman filter is used to convert the incremental data into global motion estimation, thereby achieving direct two-dimensional laser odometer measurement. Looking at the implementation process of this invention, its odometer implementation primarily employs a heuristic method rather than the traditional iterative approach to finding the optimal solution, offering advantages such as low computational cost and good real-time performance. Attached Figure Description

[0044] Figure 1 This is a flowchart illustrating the method of an embodiment of this application;

[0045] Figure 2 This is a flowchart of the serial port data stream processing for laser scanning points;

[0046] Figure 3 A schematic diagram showing the range of the localization score matrix for point pf_point;

[0047] Figure 4 This is a schematic diagram of the laser scanning point set conversion in step 3.2;

[0048] Figure 5 This is a diagram showing the search range and nearby points within that range.

[0049] Figure 6 For step 3.4, find an example graph for the relative motion of the example point point_ex;

[0050] Figure 7 A schematic diagram of adding point pairs to the rotating positioning point set matrix in step 3.5;

[0051] Figure 8 The localization score matrix for pf_point;

[0052] Figure 9 Determine the process for estimating the region;

[0053] Figure 10a The results are the global pose motion estimation test results when stationary;

[0054] Figure 10b The results are for global pose motion estimation tests at a spin rate of 15 deg / min. Detailed Implementation

[0055] like Figure 1As shown, the application provides a two-dimensional direct laser odometry calculation method applied to unmanned vehicles. The application absorbs the idea of PL-ICP, that is, the connection of scanned map points is used, but the point-to-line matching is not used, but the nearby point search method is used, thereby avoiding the matching process, reducing the demand for map features, and further estimating the mileage when the environmental features are few. For the subsequent estimated pose part, the relative motion-based positioning method is innovatively proposed to solve the pose calculation problem. The embodiments of the application specifically include:

[0056] Step 1: receiving the serial sector scan original data of the two-dimensional radar, and converting and rejecting the original data to obtain sector point set data.

[0057] Step 2: based on the static assumption, constructing an initial map and a search KD-Tree.

[0058] Step 3: after constructing the initial search map in step 2, continuously receiving the serial sector scan original data, constructing a radar coordinate system positioning score matrix and a rotation positioning point set matrix; then, according to the point set obtained by the formula conversion in step 1, constructing a dynamic search range, and obtaining map points satisfying the search range, and then based on the inverse motion assumption, updating the positioning score matrix and the rotation positioning point set matrix.

[0059] Step 4: when a frame of sector scan original data is received, estimating the incremental displacement and the incremental angular displacement according to the positioning score matrix and the rotation positioning point set matrix.

[0060] Step 5: based on kinematics, constructing an estimation system of the global pose, and then estimating the incremental displacement in step 4 to the global displacement through a low-pass filter and a Kalman filter to obtain the odometry data.

[0061] Step 6: based on the odometry calculation result in step 5, reprocessing the original point data in step 3, and updating the map and the KD-Tree.

[0062] In some embodiments, step 1. receiving the serial sector scan original data of the two-dimensional radar, and converting and rejecting the original data to obtain sector point set data. Specifically includes:

[0063] First, high-frequency sector original data of the two-dimensional radar needs to be received. The high-frequency original data can greatly improve the bandwidth of the laser odometry system and reduce the odometry delay. At the same time, since the serial original data stream is a series of 8-bit data streams, the sector point data required needs to be converted according to the protocol, and then intercepted.

[0064] The test and implementation of the embodiment are based on a Rokid M10 serial version laser radar, the system uses Ubuntu 20.04 and ROS1 Noetic. The original scanning point serial message of the radar is the original information, the frequency is relatively high, generally 200Hz-250Hz, which can greatly improve the bandwidth of the laser odometry system, reduce the odometry delay, and the interval between data is short, which can greatly reduce the error introduced by motion distortion.

[0065] At the same time, since the original scanning point data often has only a few dozen points, and is only the continuous scanning point data of a certain sector in the scanning circumference, it is difficult to calculate the pose transformation calculation using traditional iterative matching methods (such as IPC and PL-ICP), so the embodiment of the application further proposes a relative motion-based pose estimation method, which can solve the motion estimation under the condition of few data.

[0066] Then, the standardized data required for subsequent calculation need to be obtained, so the sector point data required for interception need to be converted and regularly excluded, wherein the conversion is to convert the polar coordinate point set based on the radar coordinate into a point set based on the map coordinate, which is convenient for subsequent calculation; the regular exclusion refers to distance range exclusion, which reduces the influence of far sparse points and near point high error points.

[0067] Further:

[0068] Step 1.1, radar initial serial data receiving, the process includes data receiving, data segment header search, frame tail judgment, scanning point data interception, the process is as shown in Figure 2 .

[0069] Among them, buffer is used to store the received radar message, its length should be greater than or equal to twice the maximum serial message length, so as to ensure that multiple frames of data are not lost, for M10 laser radar, the maximum frame length is 102, so it is set to 250 length. Whenever there is a received message in the serial port, it is put into rx_buffer, and searched in rx_buffer according to the radar serial port protocol, that is, the frame header 0xA5 0x5A is found first, and then the frame length is verified whether the end of the frame length is the frame tail 0xFA 0xFB, if not, the bytes other than the frame header are deleted, until the frame header is found; if so, the corresponding data segment is extracted, and the extracted serial message data segment is processed, which is based on the Rokid M10 laser radar data manual, including angle analysis, distance analysis and invalid scanning point exclusion process.

[0070] Step 1.2, for the extracted serial original data stream in step 1.1, the original point set data is converted and distance excluded, and then the standardized scanning point set required for subsequent calculation is obtained.

[0071] To realize the conversion of the parsed polar coordinate representation of the scanning point based on the radar coordinate system to the standard (x scan_point (i), y scan_point (i)) type of planar two-dimensional point set, a step of coordinate conversion is further needed, and the formula is shown in formula 1, wherein x scan_point (i) represents the x-axis coordinate of the scanning point of the i-th radar scanning point in the radar coordinate system, in meters, y scan_point (i) represents the y-axis coordinate of the scanning point of the i-th radar scanning point in the radar coordinate system, in meters, and scan_angle(i) represents the circumferential angle corresponding to the i-th radar scanning point, and scan_distance(i) represents the distance corresponding to the scanning point to the radar, in millimeters.

[0072]

[0073] The point set coordinate conversion part is to convert the standardized (x scan_point (i), y scan_point (i)) type of planar two-dimensional point set into a coordinate point set in the map coordinate system according to the previously obtained radar body position (x body , y body , w body ), denoted as (x scan_map (i), y scam_map (i)). The conversion formula is shown in formula 2, wherein x body represents the estimated x-axis coordinate of the radar body in the map coordinate system, in meters; y body represents the estimated y-axis coordinate of the radar body in the map coordinate system, in meters; and w body represents the estimated rotation angle of the radar body in the map coordinate system, in radians.

[0074]

[0075] Step 2: Based on the stationary assumption, initialize the system, and construct an initial map and a search KD-Tree.

[0076] When the search map is not initialized, the radar body position (x body , y body , w body ) is defaulted as (0, 0, 0), and the standardized planar two-dimensional point set is continuously received and converted into a map point through formula 2. When the system in the initialization state receives a specified number of planar two-dimensional point sets, the initialization flag will be changed, and the point set (x scan_map (i), y scam_map(i))After the centroid down-sampling, a KD-Tree is constructed for the set of map points used for searching, denoted as map_kdtree. The KD-Tree is a tree data structure that stores instance points in k-dimensional space for fast retrieval.

[0077] Step 3: After the map for initial search is constructed in step 2, the serial sector scan raw data is continuously received, the positioning score matrix in radar coordinate system and the rotation positioning point set matrix are constructed, then the point set is obtained according to the formula in step 1, the dynamic search range is constructed, and the map points meeting the search range are obtained, and then based on the inverse motion hypothesis, the positioning score matrix and the rotation positioning point set matrix are updated.

[0078] Firstly, according to the externally given kinematic constraint condition, the range of the radar body positioning score matrix (locate_array) and the range of the rotation positioning point set matrix (locate_point_array) are calculated.

[0079] Then, according to the received sector scan raw data, and according to the last positioning point, the scan point set is converted into map points according to formula 1 and formula 2 in step 1, and then according to the scan point set converted map points and its distance to the body, the real map point search range is constructed by fusing the two.

[0080] After that, for each scan point, the map adjacent points in its corresponding scan range are found, and then the map points meeting the search range are found.

[0081] Subsequently, according to the inverse motion hypothesis, the inverse circular arc search is performed on each map point meeting the search range, and the positioning score matrix and the rotation positioning point set matrix are updated. The overall process of step 3 can be described by the following pseudo code:

[0082]

[0083] Further:

[0084] Step 3.1, determine the range of the radar body positioning score matrix. For the motion of the radar body, its motion is mainly determined by the translation motion and the rotation motion. Since the sampling frequency is high, the sampling time is very small, so it can be approximated as a straight line motion, and its maximum motion range r is:

[0085]

[0086] where is the maximum linear velocity of the system, represents the frame interval of the radar raw data.

[0087] In order to improve the robustness of the system, a range control coefficient , where the value is greater than 1, the search range of the positioning score matrix can be expressed as:

[0088]

[0089] Let the distance resolution of the positioning score matrix be array_res, whose value is determined by the required accuracy of positioning, and the center is located at pf_point. Let the positioning score matrix be a square matrix, the number of rows is equal to the number of columns, and the size is locate_array_row*locate_array_row. Since the matrix also needs to satisfy the center at pf_point, the number of rows and columns should be odd. Finally, locate_array_row can be determined by equation 5.

[0090]

[0091] Taking point pf_point as an example, assuming that the calculated locate_array_row is 5, the positioning score matrix range of point pf_point can be obtained as Figure 3 The blue box range in the figure is the positioning score matrix range of point pf_point.

[0092] Step 3.2, according to the estimated body position (x body , y body , w body ) in the last round of calculation, which is recorded as point pf_point, the map coordinates are (pf_point.x, pf_point.y, pf_point.w), the actual application formula of equation 6 can be obtained according to equation 2 in step 1, and then the laser scanning point set is converted to the corresponding point set on the map, as shown in the figure Figure 4 .

[0093]

[0094] Step 3.3, according to the scanning point set, the map point and its distance to the body are converted, and the real map point search range is constructed by fusing them.

[0095] Let point be a point in the point set converted by the laser scanning point set on the map, then the nearest point set within the specified range min_search_r from the marked point point can be searched in the already constructed map map_kdtree. The value of min_search_r can be expressed by the function of equation 7:

[0096]

[0097] where, is a constant used to manually specify the minimum value of min_search_r; is a constant used to manually specify the maximum value of min_search_r; denotes the frame interval of radar raw data, denotes the maximum linear velocity of system running, denotes the maximum angular velocity of system running, denotes the scanning distance of point, since the frame interval of radar raw data is short, can be approximated as , and formula 8 is obtained.

[0098]

[0099] In the subsequent algorithm, it is considered that the real map point that the example point point may correspond to is in the range of . This method introduces the velocity and angular velocity properties of the estimated system into the region selection process and is associated with the sampling interval , which can ensure that the subsequent estimation process meets the actual kinematics.

[0100] For the example point point, take the calculated min_search_r as 3 map resolutions, and then obtain the search range as shown in the green filled grid in Figure 5 , and add all map points in this range to a point set, which is denoted as cl_point.

[0101] Step 3.4, determine the reverse circular arc search radius d. The radius d of the reverse circular arc search is the distance of the laser scanning point corresponding to the cl_point point set. Take the previous scanning point pf_point in Figure 3 , the example point point and the cl_point point set obtained based on point as an example, the actual scanning radius corresponding to all points in this point set should be in the numerical value:

[0102]

[0103] Iterate through the points in the cl_point point set, for each point in it, find the element that satisfies the reverse search radius d in the positioning score matrix, and increase the score corresponding to the element. Take the example point point_ex in the cl_point point set in Figure 6 as an example, the possible radar body pose origin corresponding to this point should be on the circular arc with this point as the center and d as the radius, and should be within the range of locate_array, so the coordinate points in locate_array need to satisfy:

[0104] ​

[0105] Wherein, start_x is the x starting coordinate of the positioning score matrix, end_x is the x ending coordinate of the positioning score matrix, start_y is the y starting coordinate of the positioning score matrix, and end_y is the y ending coordinate of the positioning score matrix. Taking point_ex as an example, after rounding, the following can be obtained Figure 6 The possible points of the arc search of the gray filled grid are the possible radar body pose origins.

[0106] Step 3.5, pair all the possible points found by the arc search and their original points, and put them into the rotation positioning point set matrix locate_point_array. The size of the rotation positioning point set matrix is the same as that of the positioning score matrix, and the position in the map coincides with it. The paired points are the points found by the relative motion inverse search and their corresponding map points, and the element position in the rotation positioning point set matrix is the same as the position of the motion inverse search point in the positioning score matrix. Its increase process is still taken as an example of point_ex, and the process is as shown in Figure 7 .

[0107] It is worth noting that the operations of the above steps are all performed in a loop, and the positioning score matrix and the rotation positioning point set matrix finally obtained for a laser scan are obtained by repeated loop of the above steps. In order to facilitate subsequent description, the positioning score matrix obtained after the above traversal of point_pf is given, as shown in Figure 8 .

[0108] Step 4, when a frame of sector scan original data executes all loop traversal steps of step 3, the incremental displacement is estimated according to the positioning score matrix and the rotation positioning point set matrix.

[0109] Firstly, the positioning score matrix is processed based on the maximum value threshold, and then the radar coordinate system position estimation region is screened out;

[0110] Then, the possible positions in the positioning score matrix are processed by mean value, and the translation increment of the radar is calculated; the rotation increment is calculated for all point pairs in the rotation positioning point set matrix, and the mean value of the increments is calculated to obtain the rotation increment of the radar, and the pseudo code is as shown below:

[0111]

[0112] Further:

[0113] Step 4.1, the positioning score matrix is processed based on the maximum value threshold to screen out the radar coordinate system position estimation region.

[0114] Using the maximum score of the locate_array and the given score weight as the basis for determining the estimation region, let S(x,y) be the score at position (x,y) of the locate_array. max The maximum score in locate_array

[0115]

[0116] If the score weight is denoted as weight, then all points in the location score matrix that satisfy the condition in Equation 12 are considered as the estimated region, and the estimated region is denoted as . 。:

[0117]

[0118] by Figure 8 Taking the pf_point localization score matrix as an example, the estimated region after taking a weight of 0.85 is shown below. Figure 9 As shown.

[0119] Step 4.2: Estimate the translation increment based on the estimated region. For the estimated region... The translation increment is determined by calculating the mean value through the index. The mean value position of the estimated region is shown in Equation 13:

[0120]

[0121] in This indicates the total number of points in the estimated region.

[0122] Then, based on the resolution of the positioning score matrix The translation increments delta_x and delta_y in the x and y directions can be obtained using the following formulas:

[0123]

[0124] by Figure 9 Taking the estimated region as an example, delta_x can be calculated as -1*array_res, and delta_y as -1*array_res, which means the calculation is correct. Figure 4 The translation transformation from the previous positioning point pf_point to the actual position.

[0125] Step 4.3: Calculate the incremental angular displacement estimate in the coordinate matrix of the rotational positioning point set based on the estimated region. The calculation of delta_w for the incremental angular displacement depends on the estimated region of the positioning score matrix. The subscript position in the coordinate matrix is ​​used to process several point sets in the same subscript position region in the rotation positioning point set coordinate matrix. Let the point set formed by these sequences be denoted as . .but The data in the array can be represented as:

[0126]

[0127] wherein, represents a point pair, i.e., for example Figure 7 The point pair (point_ex.x, point_ex.y) (point.x, point.y) in the array is added to the rotating positioning point set coordinate matrix.

[0128] Note The number of point pairs in the array is Then is

[0129]

[0130] Then for each point pair, the angle increment thereof can be obtained :

[0131]

[0132] For the overall incremental angular displacement delta_w, the mean value idea is still used in the embodiments of the present application, and therefore the incremental angular displacement delta_w can be obtained as shown in equation 18:

[0133]

[0134] Still taking the point pf_point as an example, the delta_w can be calculated to be about 0.0021 rad, which can reflect the rotational increment of the previous positioning point pf_point to the actual position.

[0135] Further, through qualitative analysis, when the resolution of the locate_array is fine enough, i.e., the array_res takes a smaller value, the resolution of the locate_array is higher, the circular arc reverse lookup thereof is more fine, and for the locate_point_array, the variance of the angle increment of the points contained in a single cell is also smaller, and thus a higher precision of the pose increment can be achieved, i.e., the precision upper limit of the embodiments of the present application is determined by the array_res. However, it should be noted that a smaller array_res will make the space of the locate_array larger, the data processing more, and thus the operation amount increased, the calculation time required longer, and part of the real-time lost.

[0136] Step 5: Based on kinematics, an estimation system of the global pose is constructed, and then the incremental displacement in step 4 is estimated to the global displacement through a low-pass filter and a Kalman filter, and odometer data is obtained.

[0137] Firstly, low-pass filter is used to filter the incremental data in step 4, and then a global motion estimation system based on incremental input is established, and Kalman filter is used to realize the final odometer estimation calculation.

[0138] Further:

[0139] Step 5.1, in order to realize the estimation from incremental data to global position, the embodiment of the application first uses low-pass filter to filter the incremental pose delta_x, delta_y, delta_w, low-pass filter can be used to smooth the incremental signal and reduce noise, and the embodiment of the application adopts first-order recursive low-pass filter, and the formula is shown as formula 19:

[0140]

[0141] Where y n is the filtered result, is the input this time, the value is delat_x, delat_y, delat_w, is the output of the last filtering, is the smoothing coefficient. For the filtered incremental pose, it is recorded as delat_x_fil, delat_y_fil, delat_w_fil.

[0142] Step 5.2, a global motion estimation system based on incremental input is established.

[0143] For the estimation system of global pose, each state can be expressed, and the expression contains a trigonometric function, so it can be regarded as a nonlinear system. In order to realize the estimation from incremental pose to global pose, extended Kalman is adopted. First, the state space equation of the system is established, and the uniform motion model is adopted, so that delta_x=delta_t*v_x, and the rest is similar. Therefore, the system state x=(x body ,y body ,w body ) T , and the control quantity u=(delta_x_fil, delta_y_fil, delta_w_fil).

[0144] Predicted state:

[0145]

[0146] Jacobian matrix F

[0147]

[0148] Error covariance prediction of extended Kalman

[0149]

[0150] Wherein, Q matrix is process noise covariance matrix, its value is 3x3 unit matrix multiplied by a coefficient, the coefficient is used to describe the accuracy of the motion model, the smaller the coefficient, the more accurate the model, the empirical value of the embodiment of the application is 0.01 in implementation.

[0151] Observation model

[0152]

[0153] Measurement Jacobian

[0154]

[0155] Calculate Kalman gain K:

[0156]

[0157] Wherein, R is observation noise covariance matrix, its value is 3x3 unit matrix multiplied by a coefficient, the coefficient is used to describe the trust degree of the extended Kalman filter to the measurement data, the smaller the coefficient, the more the filter depends on the observation value, the empirical value of the embodiment of the application is 0.1 in implementation.

[0158] Update state

[0159]

[0160] Update covariance

[0161]

[0162] After passing through low pass + extended Kalman, the new estimated pose (x body_new ,y body_new ,w body_new ) can be obtained, the data in some actual test is shown as Figure 10a and Figure 10b , it can be seen that the motion estimation is accurate and smooth.

[0163] Step 6, based on the odometer calculation result in step 5, reprocess the original point data in step 3, and update the map, update KD-Tree.

[0164] After obtaining the new estimated pose (x body_new ,y body_new ,w body_new ) transformation, the point operation of the map can be carried out through the following pseudo code flow.

[0165] ​​​

[0166] Further more:

[0167] Step 6.1, based on the new estimated pose (x body_new ,y body_new ,w body_new ), the scan points are converted to a set of points based on the map coordinate system again using formula 28,

[0168]

[0169] Step 6.2, for the converted point set, all points therein are traversed, and for each point, a KD-Tree is used for a nearby point search, if there is no map point in a specified range min_dis_lim, the point is considered as a new point of the map and needs to be added to the map. This scheme only operates on the number of scan point clouds each time, rather than the entire point cloud, and can avoid dense map point clouds through distance limitation, and has the advantages of high efficiency and speed.

[0170] Step 6.3, the KD-Tree reconstruction is triggered by a 10Hz timer, and the search KD-Tree is reconstructed based on the updated map in step 6.2, so as to realize the extension of the odometer checkpoint map range.

[0171] Based on the same concept of the above method, the embodiment of the application further provides a two-dimensional direct laser odometer calculation system applied to an unmanned vehicle, comprising:

[0172] a data receiving and preprocessing module, configured to:

[0173] receive high-frequency sector scan original data of a two-dimensional laser radar;

[0174] convert and eliminate the original data to obtain standardized sector point set data;

[0175] an initial map construction module, configured to:

[0176] construct an initial map and a search KD-Tree based on a stationary assumption;

[0177] a dynamic search and positioning score matrix updating module, configured to:

[0178] continuously receive sector scan original data, construct a radar coordinate system positioning score matrix and a rotating positioning point set matrix;

[0179] update the positioning score matrix and the rotating positioning point set matrix according to a reverse motion assumption;

[0180] an incremental pose estimation module, configured to:

[0181] According to the positioning score matrix and the rotation positioning point set matrix, an incremental displacement and an incremental angular displacement are estimated;

[0182] A global pose estimation module is configured to:

[0183] Based on a kinematic model, the incremental displacement is estimated to a global displacement through a low-pass filter and a Kalman filter to obtain odometer data.

[0184] A map updating module is configured to:

[0185] According to the global pose estimation result, the map and the KD-Tree are updated.

[0186] The implementation processes of the functions and roles of the modules in the above device are specifically described in the implementation processes of the corresponding steps in the above method, and will not be repeated here.

[0187] Embodiments of the present application also provide an electronic device. At the hardware level, the electronic device includes a processor, an internal bus, a network interface, a memory and a non-volatile memory, and of course, other hardware required by other services. The processor reads the corresponding computer program from the non-volatile memory into the memory and then runs. Of course, in addition to the software implementation, the present application does not exclude other implementation manners, such as a logic device or a combination of software and hardware, and the like. That is, the execution subject of the following processing flow is not limited to each logic unit, but can also be hardware or a logic device.

[0188] Embodiments of the present application also provide a computer readable storage medium, which stores a computer program. The program is executed by a processor to implement a laser odometer calculation method as shown in any of the preceding embodiments.

[0189] It should be further noted that the terms "comprising", "containing" or any other variant thereof are intended to cover non-exclusive inclusion, so that processes, methods, articles or devices including a series of elements not only include those elements, but also include other elements not explicitly listed or inherent to such processes, methods, articles or devices. Without more limitations, the element defined by the statement "including a" does not exclude the presence of other identical elements in the process, method, article or device including the element.

[0190] The above only describes the preferred embodiments of the present application and does not limit the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the scope of protection of the present application.

Claims

1. A two-dimensional direct laser odometry calculation method applied to unmanned vehicles, characterized by, The method comprises the following steps: Data receiving and preprocessing: Receiving high-frequency sector scanning raw data of a two-dimensional laser radar; Converting and rejecting the raw data to obtain standardized sector point set data; Initial map construction: Based on the assumption of static, constructing an initial map and a search KD-Tree; Dynamic search and positioning score matrix updating: Continuously receiving sector scanning raw data, constructing a radar coordinate system positioning score matrix and a rotating positioning point set matrix; According to the assumption of reverse motion, updating the positioning score matrix and the rotating positioning point set matrix; Incremental pose estimation: According to the positioning score matrix and the rotating positioning point set matrix, estimating incremental displacement and incremental angular displacement; Global pose estimation: Based on the kinematic model, estimating the incremental displacement to global displacement through a low-pass filter and a Kalman filter to obtain odometer data; Map updating: According to the global pose estimation result, updating the map and the KD-Tree.

2. The method for two-dimensional direct laser odometry calculation for unmanned vehicles according to claim 1, characterized in that, In the data receiving and preprocessing step: The frequency of the received high-frequency sector scanning raw data of the two-dimensional laser radar is 200Hz-250Hz; Converting the raw data includes converting the polar coordinate point set based on the radar coordinate into a point set based on the map coordinate; Rejecting the raw data includes rejecting points out of the distance range, reducing the influence of far sparse points and near high error points.

3. The method for two-dimensional direct laser odometry calculation for unmanned vehicles according to claim 1 or 2, characterized in that, In the initial map construction step: Based on the assumption of static, the radar body position is defaulted as (0, 0, 0); The initial map is constructed through centroid downsampling, and the map point set KD-Tree for search is denoted as map_kdtree. 4.The method for two-dimensional direct laser odometry calculation applied to unmanned vehicles according to claim 3, wherein, In the dynamic search and positioning score matrix updating step: The range of the positioning score matrix is calculated according to the kinematic constraint conditions of the radar, including translational motion and rotational motion; The dynamic search range is constructed according to the conversion of the scanning point set into the map point and the distance thereof to the body; The reverse circular arc lookup radius d is the distance of the laser scanning point corresponding to the map point. 5.The method for two-dimensional direct laser odometry calculation applied to unmanned vehicles according to claim 1, wherein, In the incremental pose estimation step: The positioning score matrix is processed through the maximum value threshold to filter out the radar coordinate system position estimation region; The possible positions in the positioning score matrix are processed by mean value to calculate the translational increment of the radar; The rotation increment of the points in all possible positions in the rotating positioning point set matrix is calculated, and the increment is averaged to calculate the rotation increment of the radar. 6.The method for two-dimensional direct laser odometry calculation applied to unmanned vehicles according to claim 5, wherein, In the global pose estimation step: The incremental pose is filtered using a low-pass filter to reduce noise; A global motion estimation system based on incremental input is established, and an extended Kalman filter is used to realize the final odometer estimation calculation.

7. The method for two-dimensional direct laser odometry calculation for unmanned vehicles according to claim 1 or 6, characterized in that, In the map updating step: According to the newly estimated pose, the scanning point is converted into a point set based on the map coordinate system again; The KD-Tree is used for nearby point search, and if there is no map point within the specified range, the point is added to the map; The KD-Tree is updated at a frequency of 10Hz to realize dynamic updating of the map.

8. A two-dimensional direct laser odometry calculation system applied to an unmanned vehicle, characterized by, It comprises: A data receiving and preprocessing module, configured to: Receive high-frequency sector scanning raw data of a two-dimensional laser radar; Convert and reject the raw data to obtain standardized sector point set data; An initial map construction module, configured to: constructing an initial map and searching a KD-Tree based on a static hypothesis; a dynamic search and localization score matrix updating module, configured to: continuously receive sector scanning raw data, and construct a radar coordinate system localization score matrix and a rotating localization point set matrix; update the localization score matrix and the rotating localization point set matrix according to a reverse motion hypothesis; an incremental pose estimation module, configured to: estimate an incremental displacement and an incremental angular displacement according to the localization score matrix and the rotating localization point set matrix; a global pose estimation module, configured to: estimate a global displacement from the incremental displacement through a low-pass filter and a Kalman filter based on a kinematic model, to obtain odometer data; a map updating module, configured to: update the map and the KD-Tree according to the global pose estimation result.

9. An electronic device, comprising: comprise: a processor; a memory for storing processor-executable instructions; wherein the processor implements the method of any one of claims 1-7 by running the executable instructions. The instructions, when executed by the processor, implement the steps of the method of any one of claims 1-7.

10. A computer readable storage medium having stored thereon computer instructions, wherein, ​ ​

Images