Laser positioning method, computer device and storage medium

Abstract

The present application provides a laser positioning method, a computer device, and a storage medium. When screening initial extreme values through a correlation scanning and matching method, by matching each laser point with a weight coefficient corresponding to its respective laser distance, and then calculating a matching score in combination with this weight coefficient, the matching scores corresponding to each value are related to the ranging distance, thereby eliminating the influence of the ranging distance on the matching score, so that the initial extreme value with the highest matching score finally screened has high accuracy, and finally ensuring the accuracy of the global extreme value obtained after optimizing by the Newton-Gauss algorithm based on this initial extreme value.

Description

Technical Field

[0001] This application relates to the technical field of mapping and positioning, and particularly relates to a laser positioning method, a computer device, and a storage medium. Background Art

[0002] A complete 2D laser SLAM solution includes two major parts: front-end robot positioning and back-end optimization. For a positioning method that fuses IMU, odometer, and lidar, the robot pose estimation is generally completed through frame-to-frame matching. First, the pose estimation of the robot in the next frame is obtained through the odometer as a prior, and then the prior is corrected by the observed data of laser matching to output the posterior pose. Commonly used laser matching methods include ICP-like methods and optimization-based methods. However, the optimization-based methods are very sensitive to the initial values. If the initial values are not provided well, it is easy to fall into local extrema, resulting in large errors in positioning.

[0003] For a floor sweeper, the cost of the lidar is relatively low, but the laser data is limited by the accuracy of the lidar. As the ranging distance increases, when it exceeds a certain range, the accuracy of the laser data will decrease. Therefore, when using low-precision laser data for mapping and positioning of the floor sweeper, the laser data as the initial value will result in low positioning accuracy of the floor sweeper. Summary of the Invention

[0004] The main purpose of this application is to provide a laser positioning method, a computer device, and a storage medium, aiming to solve the drawback that the accuracy of the laser data of the existing lidar decreases after exceeding a certain range, resulting in low positioning accuracy of the floor sweeper.

[0005] To achieve the above purpose, this application provides a laser positioning method, including:

[0006] Collecting laser data, where the laser data includes a plurality of laser points;

[0007] Finding rough extrema from the laser data through a correlation scan matching method, and performing a preset screening operation on the rough extrema to obtain initial extrema;

[0008] Using the Gauss-Newton algorithm to optimize the initial extrema, and converging to obtain global extrema, where the global extrema represent the positioning pose.

[0009] This application also provides a laser positioning device, including:

[0010] A collecting module, configured to collect laser data, where the laser data includes a plurality of laser points;

[0011] A screening module, configured to find rough extrema from the laser data through a correlation scan matching method, and perform a preset screening operation on the rough extrema to obtain initial extrema;

[0012] An optimization module, configured to optimize the initial extreme value using the Gauss-Newton algorithm to converge to a global extreme value, where the global extreme value represents the positioning pose.

[0013] This application also provides a computer device, including a memory and a processor. A computer program is stored in the memory, and when the processor executes the computer program, the steps of the method described in any one of the above are implemented.

[0014] This application also provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the method described in any one of the above are implemented.

[0015] A laser positioning method, computer device, and storage medium provided in this application first collect laser data, which includes multiple laser points. Then, rough extreme values are found from the laser data through the correlation scanning matching method, and further a preset screening operation is performed on the rough extreme values to obtain initial extreme values. Finally, the Gauss-Newton algorithm is used to optimize the initial extreme values to converge to a global extreme value, and this global extreme value represents the positioning pose of the sweeping robot. When this application screens the initial extreme values through CSM (correlation scanning matching method), by matching each laser point with a weight coefficient corresponding to its respective laser distance, and then calculating the matching score in combination with this weight coefficient, so that the matching scores corresponding to each value are related to the ranging distance, thereby eliminating the influence of the ranging distance on the matching score, making the initial extreme value with the highest matching score finally screened have high accuracy, and finally ensuring the accuracy of the global extreme value obtained after optimizing the Newton-Gauss algorithm based on this initial extreme value. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 is a schematic diagram of the steps of the laser positioning method in an embodiment of this application;

[0017] Figure 2 is a block diagram of the overall structure of the laser positioning device in an embodiment of this application;

[0018] Figure 3 is a schematic block diagram of the structure of the computer device in an embodiment of this application.

[0019] The realization, functional features, and advantages of the purpose of this application will be further described with reference to the embodiments and the accompanying drawings. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0020] In order to make the purpose, technical solutions, and advantages of this application clearer, the following further details this application with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain this application and are not used to limit this application.

[0021] Reference Figure 1 , in an embodiment of the present application, a laser positioning method is provided, including:

[0022] S1: Collect laser data, where the laser data includes multiple laser points;

[0023] S2: Find rough extreme values from the laser data through a correlation scanning and matching method, perform a preset screening operation on the rough extreme values to obtain initial extreme values;

[0024] S3: Optimize the initial extreme values using the Gauss-Newton algorithm, and converge to obtain global extreme values, where the global extreme values represent the positioning pose.

[0025] In this embodiment, the laser positioning method applied to a sweeping robot is taken as an example for specific illustration. A lidar is deployed on the sweeping robot, and the positioning system of the sweeping robot collects laser data through the lidar. The laser data includes multiple laser points. The positioning system finds extreme values from the laser data of the current frame through CSM. CSM is a brute-force search that calculates the matching scores for all solutions (or poses) in the solution space and screens out several values with matching scores within a preset sorting range as initial extreme values. In this embodiment, based on the existing CSM calculation logic, the weight calculation of the laser distance is integrated; specifically, the positioning system first obtains the pose increment between two adjacent frames from the odometer sequence of the sweeping robot and uses this pose increment as a matching prior, and then calculates the current frame pose of the sweeping robot based on the pose increment as the matching prior and the previous frame pose of the sweeping robot. Then, with the known translational resolution R t , angular resolution R r , the two side lengths L w and L H of the search window, a search space S is constructed. The formula is specifically:

[0026] The positioning system constructs a likelihood field based on the laser data / local map data of the previous frame. The construction method is specifically: Step1: Establish a likelihood model, assuming that the probability distribution of the laser point hits follows a Gaussian distribution of N(0,Σ); Step2: Establish a likelihood field of size N*N centered on the position of the sweeping robot in the previous frame, store it in the table Tb[N][N], and initialize it to 0. The resolution of the likelihood field is the same as the resolution of the grid map. Assume that the current grid resolution is R g ; Step3: Project each laser point into the table Tb[N][N], and update the likelihood values in the likelihood field with the above Gaussian model. The update formula is as follows: r = d*cos(a) / R g , c = d*sin(a) / R g , Update(Tb): Where d is the current laser point distance, a is the current laser point angle, r and c are the coordinates of the laser point in the likelihood field, and x stores the grid coordinates of all laser points in the previous frame of laser data.

[0027] The positioning system obtains the laser distance corresponding to each laser point, and then filters from the preset mapping table to obtain the weight coefficient corresponding to each laser distance. Next, the positioning system retrieves the laser angle of each laser point and the grid resolution of the grid map, and substitutes the grid resolution of the grid map, the laser distance, laser angle, and weight coefficient corresponding to each laser point into the pre-constructed scoring function for calculation, so as to obtain the matching score corresponding to each laser point (each laser point represents a solution of a resolution of CSM).

[0028] The positioning system uses the Gauss-Newton algorithm to optimize the initial extreme value obtained by searching through CSM, and converges to obtain the global extreme value, which represents the positioning pose. Specifically, the positioning system first filters out the noise points (outliers that may be caused by noise or dynamic obstacles) in the laser data of the current frame based on the likelihood value, and then uses GN (Gauss-Newton algorithm) to optimize the initial extreme value after filtering out the noise points, so as to obtain the global extreme value T final , thus completing the matching between the current frames. The optimization process of GN is as follows:

[0029] Step1: Construct the error model: E(T) = argmin∑(1 - M(S i (T))) 2 , where is the current solution, S i is the projection of the i-th point according to T, and M(Si(T)) is the likelihood value of this point. Since some outliers have been removed in the previous step, the calculation error here will be reduced due to the extra residuals brought by the outliers;

[0030] Step2: Linearize M(x) and perform the first-order Taylor expansion: E(T + ΔT) = argmin∑(1 - M(S i (T + ΔT))) 2 ; Step3: Take the derivative of ΔT and set the derivative to 0 to obtain ΔT: E(T + ΔT) = argmin∑(1 - M(S i (T + ΔT))) 2 ,

[0031]

[0032]

[0033]

[0034]

[0035] Step 4: Update the input pose T. If the iteration termination condition is met, the iteration ends; otherwise, go to

[0036] Step 3. Thus, the entire laser positioning process steps are completed, and the global extreme value representing the positioning pose is obtained.

[0037] In this embodiment, when screening the initial extreme value through CSM (Correlation Scan Matching method), by matching the weight coefficient corresponding to the respective laser distance for each laser point, and then calculating the matching score in combination with this weight coefficient, the matching scores corresponding to each value are related to the ranging distance. Furthermore, the influence of the ranging distance on the matching score is excluded, so that the initial extreme value with the highest matching score finally screened has high accuracy, and finally the accuracy of the global extreme value obtained after optimizing by the Newton-Gauss algorithm based on this initial extreme value is ensured.

[0038] Furthermore, the preset screening operation for the rough extreme value to obtain the initial extreme value includes:

[0039] S201: Perform weighted calculation of the matching score for the laser distance corresponding to each rough extreme value, and screen out the initial extreme value whose matching score is within the preset sorting range; and / or,

[0040] S202: Dynamically filter the laser points corresponding to each rough extreme value, and screen out the initial extreme value whose likelihood value is not less than the likelihood value threshold from each of the laser points.

[0041] In one embodiment, the calculation logic of the matching score corresponding to each rough extreme value by the positioning system is the same. Taking the matching score of the pose corresponding to a single rough extreme value as an example for illustration. The positioning system first obtains the laser distance of the current laser point corresponding to this rough extreme value, and then screens out the corresponding weight coefficient from the preset mapping table according to this laser distance; wherein, the preset mapping table includes multiple groups of corresponding laser distance ranges and weight coefficients. The positioning system retrieves the laser angle of the current laser point and the grid resolution of the grid map, and substitutes the laser distance, laser angle, grid resolution, and weight coefficient of the current laser point into the pre-constructed scoring function for calculation to obtain the matching score. According to the above calculation logic of the matching score, the positioning system performs weighted calculation on the laser distance corresponding to each rough extreme value to obtain the matching score corresponding to each rough extreme value. Then, screen out the rough extreme values whose matching scores are within the preset sorting range as the initial extreme values; for example, the top 5 rough extreme values in the sorting are the 5 initial extreme values selected this time.

[0042] In another embodiment, the positioning system projects the rough extreme values into a pre-constructed likelihood field, so as to query the likelihood values corresponding to each laser point of the laser data of the current frame; wherein, the likelihood field is constructed according to the laser data of the previous frame. Then, the positioning system retrieves the likelihood value threshold, and filters out the outliers with likelihood values less than the likelihood value threshold among each laser point, so as to obtain a number of matching laser points; wherein, the selected matching laser points are used as the initial extreme values after screening.

[0043] In this embodiment, step S201 and step S202 can be applied separately or in combination in actual applications; when step S201 and step S202 are applied in combination, the execution order between them is not specifically limited.

[0044] Further, the calculation steps of the matching score corresponding to a single pose include:

[0045] S2011: Obtain the laser distance of the current laser point;

[0046] S2012: Screen out the corresponding weight coefficient from the preset mapping table according to the laser distance, and the preset mapping table includes multiple groups of corresponding laser distance ranges and weight coefficients;

[0047] S2013: Retrieve the laser angle of the current laser point and the grid resolution of the grid map, and substitute the laser distance, the laser angle, the grid resolution and the weight coefficient into a pre-constructed score function for calculation to obtain the matching score.

[0048] In this embodiment, the positioning system obtains the laser distance of the current laser point, and then screens out the corresponding weight coefficient from the preset mapping table according to the laser distance. Among them, the preset mapping table includes multiple groups of corresponding laser distance ranges and weight coefficients. When d (laser distance) < 4m, the weight coefficient w i is 1; when 4m < d < 6m, the weight coefficient w i is 0.8; when d > 6m, the weight coefficient w i is 0.5. The positioning system retrieves the laser angle of the current laser point and the grid resolution of the grid map, and then substitutes the laser distance, the laser angle, the grid resolution and the corresponding weight coefficient into a pre-constructed score function for calculation, so as to obtain the required matching score. Among them, the score function is specifically: Score(T) is the matching score, N is the number of laser points, R g is the grid resolution, and a is the laser angle.

[0049] Further, the step of dynamically filtering the laser points corresponding to the rough extreme values and screening the initial extreme values with a likelihood value not less than the likelihood value threshold from each of the laser points includes:

[0050] S2021: Project the rough extreme values into a pre-constructed likelihood field, and query the likelihood values corresponding to each of the laser points of the laser data of the current frame, where the likelihood field is constructed according to the laser data of the previous frame;

[0051] S2022: Retrieve the likelihood value threshold, and filter out the outliers among the laser points with likelihood values less than the likelihood value threshold to obtain a number of matching laser points; where the matching laser points are used as the initial extreme values after screening.

[0052] In this embodiment, the positioning system projects the rough extreme values into a pre-constructed likelihood field, and queries the likelihood values corresponding to each of the laser points included in the laser data of the current frame; where the likelihood field is constructed according to the laser data of the previous frame. The positioning system retrieves the preset likelihood value threshold, and compares the likelihood values corresponding to each of the laser points with the likelihood value threshold to determine the size relationship between the two. If the likelihood value of a laser point is less than the likelihood value threshold, it means that the laser point is an outlier caused by noise or a dynamic obstacle. To ensure the optimization accuracy of the subsequent Gauss-Newton algorithm, the positioning system filters out the outliers among the laser points with likelihood values greater than the likelihood value threshold. The remaining laser points after filtering are the matching laser points, and these matching laser points are used as the initial extreme values after screening.

[0053] Further, the laser positioning method is applied to a sweeping robot, and the sweeping robot is equipped with a lidar and a gyroscope. The step of collecting laser data includes:

[0054] S101: Collect initial laser data through the lidar, and determine whether the sweeping robot tilts during the collection of the initial laser data;

[0055] S102: If the sweeping robot tilts during the collection of the initial laser data, determine whether the environment where the sweeping robot is located belongs to a first preset environment or a second preset environment during the collection of the initial laser data;

[0056] S103: If the environment where the sweeping robot is located belongs to the first preset environment during the collection of the initial laser data, calculate the first laser ranging distance and the second laser ranging distance from the laser emitted by the lidar to the bearing surface through the gyroscope data of the gyroscope, where the bearing surface is the surface supporting the sweeping robot;

[0057] S104: Filter the candidate laser data within the preset range of the first laser ranging distance and the second laser ranging distance in the initial laser data to obtain the laser data;

[0058] S105: If the environment where the sweeper is located belongs to the second preset environment during the process of collecting the initial laser data, then use the initial laser data as the laser data.

[0059] In this embodiment, a gyroscope is also deployed on the sweeper. The positioning system collects the initial laser data through the lidar and determines whether the sweeper tilts during the process of collecting the initial laser data based on the gyroscope data collected by the gyroscope. If the sweeper tilts during the process of collecting the initial laser data, it is necessary to further determine whether the environment where the sweeper is located belongs to the first preset environment or the second preset environment according to the change range of the gyroscope data; among them, the first preset environment is specifically the threshold environment, and the second preset environment is specifically the ramp environment. If the change range of the gyroscope data is greater than the threshold, it is determined that the environment where the sweeper is located belongs to the first preset environment during the process of collecting the initial laser data, and in this case, some of the data in the collected initial laser data has low confidence and needs to be removed. Specifically, the positioning system calculates the first laser ranging distance and the second laser ranging distance from the laser emitted by the lidar to the bearing surface when collecting the laser data of the current frame through the gyroscope data; where the bearing surface is the surface that supports the sweeper, usually the ground. Filter out all the laser points (i.e., candidate laser data) in the initial laser data whose laser distances corresponding to the laser points are within the preset range of the first laser ranging distance and the second laser ranging distance, so as to obtain laser data with higher confidence to ensure the data accuracy during subsequent processing. If the change range of the gyroscope data is less than the threshold, it is determined that the environment where the sweeper is located belongs to the second preset environment during the process of collecting the initial laser data. At this time, the jitter of the sweeper during movement is small, and the stability of the collected laser data is strong, so the confidence is high; the positioning system directly uses the initial laser data as the required laser data for the next step of processing.

[0060] Further, the step of determining whether the environment where the sweeper is located belongs to the first preset environment or the second preset environment during the process of collecting the initial laser data includes:

[0061] S10201: Retrieve the gyroscope data, and the gyroscope data includes multiple gyroscope values;

[0062] S10202: Calculate the standard deviation of the gyroscope data according to each gyroscope value;

[0063] S10203: Determine whether the standard deviation is greater than the standard deviation threshold;

[0064] S10204: If the standard deviation is greater than the standard deviation threshold, it is determined that the environment where the sweeping robot is located belongs to the first preset environment during the process of collecting the initial laser data;

[0065] S10205: If the standard deviation is less than the standard deviation threshold, it is determined that the environment where the sweeping robot is located belongs to the second preset environment during the process of collecting the initial laser data.

[0066] In this embodiment, the positioning system retrieves the gyroscope data during the process of the sweeping robot collecting the initial laser. The gyroscope data includes multiple gyroscope values, and each gyroscope data is arranged in the order of collection time. The positioning system calculates according to the calculation logic of the standard deviation based on each gyroscope value, so as to obtain the standard deviation of the gyroscope data, and this standard deviation characterizes the degree of dispersion of each gyroscope value in the gyroscope data. The positioning system retrieves the preset standard deviation threshold and compares the standard deviation of the gyroscope data with the standard deviation threshold to determine the magnitude relationship between the two. If the standard deviation of the gyroscope data is greater than the standard deviation threshold, it indicates that the sweeping robot is relatively bumpy during the process of collecting the initial laser data, and the environment where the sweeping robot is located belongs to the first preset environment, that is, the environment where the sweeping robot climbs over the threshold. If the standard deviation of the gyroscope data is less than the standard deviation threshold, it indicates that each gyroscope value is relatively stable, and further indicates that the sweeping robot is in a relatively stable motion environment as a whole. Therefore, the positioning system determines that the environment where the sweeping robot is located belongs to the second preset environment during the process of collecting the initial laser data.

[0067] Further, the step of calculating the first laser ranging distance and the second laser ranging distance from the laser emitted by the lidar to the bearing surface through the gyroscope data of the gyroscope includes:

[0068] S10401: Analyze the first tilt angle and the second tilt angle of the sweeping robot through the gyroscope data. The first tilt angle is the tilt angle of the sweeping robot on the Y-axis currently, and the second tilt angle is the tilt angle of the sweeping robot around the X-axis currently;

[0069] S10402: Obtain the height of the lidar from the ground, and calculate the first laser ranging distance according to the height from the ground and the first tilt angle, and calculate the second laser ranging distance according to the height from the ground and the second tilt angle.

[0070] In this embodiment, the positioning system estimates the current first tilt angle of the sweeper in the Y-axis and the second tilt angle around the X-axis through the gyroscope data of the sweeper; wherein, the direction of the horizontal plane is the X-axis direction, and the direction perpendicular to the horizontal plane is the Y-axis direction. The positioning system obtains the height of the lidar from the ground, and substitutes the height from the ground and the first tilt angle into the first calculation formula: D p_min = H / |sinθ pitch |, and calculates the first lidar ranging distance, where D p_min is the first lidar ranging distance, H is the height from the ground, and θ pitch is the first tilt angle. Substitute the height from the ground and the second tilt angle into the second calculation formula: D r_min = H / |sinθ roll |, and calculates the second lidar ranging distance, where D r_min is the second lidar ranging distance, and θ roll is the second tilt angle.

[0071] Further, before the step of collecting lidar data, it includes:

[0072] S4: Detect whether the sweeper collides during movement through the collision sensor;

[0073] S5: If the sweeper does not collide during movement, obtain the pose increment between two adjacent frames of odometer data from the odometer sequence based on time sequence, and use the pose increment as the prior pose increment;

[0074] S6: If the sweeper collides during movement, retrieve a preset value and set the preset value as the prior pose increment;

[0075] S7: Retrieve the previous frame pose of the sweeper, and calculate the current frame pose of the sweeper according to the previous frame pose and the prior pose increment.

[0076] In this embodiment, a collision sensor is deployed on the floor sweeper. The positioning system detects whether a collision occurs during the movement of the floor sweeper (i.e., during the process of collecting laser data) through the collision sensor. If no collision occurs during the movement of the floor sweeper, the pose increment between two adjacent frames of odometer data is obtained from the odometer sequence based on the time sequence (i.e., the acquisition time order of the odometer data), and this pose increment is used as the prior pose increment. If a collision occurs during the movement of the floor sweeper, it indicates that the wheels of the floor sweeper slip, and the floor sweeper should be stationary or only move a very short distance. If the pose increment between two adjacent frames of odometer data is directly used as the initial value, the subsequent optimization scheme of the Gauss-Newton algorithm cannot converge to the global extreme value. Therefore, the positioning system retrieves a preset value, which is preferably 0, and sets this preset value as the prior pose increment for the current time. Further, the positioning system retrieves the previous pose of the floor sweeper and calculates based on the previous pose and the prior pose increment to obtain the current pose of the floor sweeper.

[0077] Referring to Figure 2 , an embodiment of the present application further provides a laser positioning device, including:

[0078] An acquisition module 1 for acquiring laser data, where the laser data includes multiple laser points;

[0079] A screening module 2 for finding rough extreme values from the laser data through a correlation scanning and matching method, performing a preset screening operation on the rough extreme values to obtain initial extreme values;

[0080] An optimization module 3 for optimizing the initial extreme values using the Gauss-Newton algorithm to converge to the global extreme value, where the global extreme value represents the positioning pose.

[0081] Further, the screening module 2 includes:

[0082] A first calculation unit for performing weighted calculation of the matching scores for the laser distances corresponding to the rough extreme values respectively, and screening to obtain the initial extreme values with the matching scores within a preset sorting range; and / or,

[0083] A filtering unit for dynamically filtering the laser points corresponding to the rough extreme values respectively, and screening to obtain the initial extreme values with the likelihood values not less than the likelihood value threshold from each of the laser points.

[0084] Further, the calculation unit includes:

[0085] An acquisition subunit for acquiring the laser distance of the current laser point;

[0086] A screening subunit, configured to screen a corresponding weight coefficient from a preset mapping table according to the laser distance, where the preset mapping table includes multiple groups of laser distance ranges and weight coefficients that correspond one by one;

[0087] A first calculation subunit, configured to retrieve the laser angle of the current laser point and the grid resolution of the grid map, and substitute the laser distance, the laser angle, the grid resolution, and the weight coefficient into a pre-constructed scoring function for calculation to obtain the matching score.

[0088] Further, the filtering unit includes:

[0089] A query subunit, configured to project the rough extreme value into a pre-constructed likelihood field, and query the likelihood value corresponding to each of the laser points of the laser data of the current frame, where the likelihood field is constructed according to the laser data of the previous frame;

[0090] A filtering subunit, configured to retrieve a likelihood value threshold, and filter out the outlier points among the laser points where the likelihood value is less than the likelihood value threshold to obtain a plurality of matching laser points; wherein, the matching laser points are used as the initial extreme values after screening.

[0091] Further, the laser positioning method is applied to a floor sweeper, and the floor sweeper is equipped with a lidar and a gyroscope. The acquisition module 1 includes:

[0092] A first judgment unit, configured to collect initial laser data through the lidar, and judge whether the floor sweeper tilts during the acquisition process of the initial laser data;

[0093] A second judgment unit, configured to, if the floor sweeper tilts during the acquisition process of the initial laser data, judge whether the environment where the floor sweeper is located during the acquisition process of the initial laser data belongs to a first preset environment or a second preset environment;

[0094] A second calculation unit, configured to, if the environment where the floor sweeper is located during the acquisition process of the initial laser data belongs to the first preset environment, calculate a first laser ranging distance and a second laser ranging distance from the laser emitted by the lidar to the bearing surface through the gyroscope data of the gyroscope, where the bearing surface is the surface supporting the floor sweeper;

[0095] A filtering unit, configured to filter the candidate laser data within a preset range of the first laser ranging distance and the second laser ranging distance in the initial laser data to obtain the laser data;

[0096] A setting unit, configured to use the initial laser data as the laser data if the environment where the sweeper is located belongs to the second preset environment during the process of collecting the initial laser data.

[0097] Further, the second determination unit includes:

[0098] An acquisition subunit, configured to acquire the gyroscope data, where the gyroscope data includes a plurality of gyroscope values;

[0099] A second calculation subunit, configured to calculate the standard deviation of the gyroscope data according to each of the gyroscope values;

[0100] A judgment subunit, configured to judge whether the standard deviation is greater than a standard deviation threshold;

[0101] A first determination subunit, configured to determine that the environment where the sweeper is located belongs to the first preset environment during the process of collecting the initial laser data if the standard deviation is greater than the standard deviation threshold;

[0102] A second determination subunit, configured to determine that the environment where the sweeper is located belongs to the second preset environment during the process of collecting the initial laser data if the standard deviation is less than the standard deviation threshold.

[0103] Further, the second calculation unit includes:

[0104] An analysis subunit, configured to analyze the first tilt angle and the second tilt angle of the sweeper from the gyroscope data, where the first tilt angle is the tilt angle of the sweeper on the Y axis currently, and the second tilt angle is the tilt angle of the sweeper around the X axis currently;

[0105] A third calculation subunit, configured to obtain the height of the lidar from the ground, and calculate a first laser ranging distance according to the height from the ground and the first tilt angle, and calculate a second laser ranging distance according to the height from the ground and the second tilt angle.

[0106] Further, the laser positioning device further includes:

[0107] A detection module 4, configured to detect whether the sweeper collides during the movement process through the collision sensor;

[0108] An acquisition module 5, configured to, if the sweeper does not collide during the movement process, acquire a pose increment between two adjacent frames of odometer data from the odometer sequence based on time sequence, and use the pose increment as a prior pose increment;

[0109] A setting module 6, configured to retrieve a preset value and set the preset value as the prior pose increment if the floor cleaner collides during movement.

[0110] A calculation module 7, configured to retrieve the previous frame pose of the floor cleaner and calculate the current frame pose of the floor cleaner according to the previous frame pose and the prior pose increment.

[0111] In this embodiment, each module, unit, and subunit in the laser positioning device is used to correspondingly execute each step in the above laser positioning method, and the specific implementation process thereof will not be elaborated herein.

[0112] A laser positioning device provided in this embodiment first collects laser data, and the laser data includes multiple laser points. Then, extreme values are searched from the laser data through the correlation scan matching method, and during the process of searching for extreme values, weighted calculation of the matching scores is performed based on the laser distances respectively corresponding to each laser point, so as to screen out the initial extreme value with the highest matching score. Finally, the Gauss-Newton algorithm is used to optimize the initial extreme value, and the global extreme value is converged, and the global extreme value represents the positioning pose of the floor cleaner. When this application screens the initial extreme value through CSM (correlation scan matching method), by matching each laser point with the weight coefficient corresponding to its respective laser distance, and then calculating the matching score in combination with the weight coefficient, the matching scores corresponding to each value are related to the ranging distance, thereby eliminating the influence of the ranging distance on the matching score, so that the finally screened initial extreme value with the highest matching score has high accuracy, and finally ensures the accuracy of the global extreme value obtained after optimizing the Newton-Gauss algorithm based on the initial extreme value.

[0113] Refer to Figure 3 , a computer device is further provided in an embodiment of this application. The computer device may be a server, and its internal structure may be as Figure 3 shown. The computer device includes a processor, a memory, a network interface, and a database connected through a system bus. Among them, the processor of the computer design is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store data such as laser data. The network interface of the computer device is used to communicate with an external terminal through a network connection. The computer program, when executed by the processor, implements a laser positioning method.

[0114] The above processor executes the steps of the above laser positioning method:

[0115] S1: Collect laser data, and the laser data includes multiple laser points;

[0116] S2: Find rough extreme values from the laser data through a correlation scanning and matching method, perform a preset screening operation on the rough extreme values, and obtain initial extreme values;

[0117] S3: Use the Gauss-Newton algorithm to optimize the initial extreme values, and converge to obtain global extreme values, where the global extreme values represent the positioning pose.

[0118] Further, the performing a preset screening operation on the rough extreme values to obtain initial extreme values includes:

[0119] S201: Perform weighted calculation of matching scores for the laser distances corresponding to the rough extreme values respectively, and screen to obtain the initial extreme values whose matching scores are within a preset sorting range; and / or,

[0120] S202: Perform dynamic filtering on the laser points corresponding to the rough extreme values respectively, and screen from each of the laser points to obtain the initial extreme values whose likelihood values are not less than the likelihood value threshold.

[0121] Further, the calculation steps of the matching score corresponding to a single pose include:

[0122] S2011: Obtain the laser distance of the current laser point;

[0123] S2012: Screen the corresponding weight coefficient from a preset mapping table according to the laser distance, where the preset mapping table includes multiple groups of corresponding laser distance ranges and weight coefficients;

[0124] S2013: Retrieve the laser angle of the current laser point and the grid resolution of the grid map, and substitute the laser distance, the laser angle, the grid resolution, and the weight coefficient into a pre-constructed scoring function for calculation to obtain the matching score.

[0125] Further, the step of performing dynamic filtering on the laser points corresponding to the rough extreme values respectively and screening from each of the laser points to obtain the initial extreme values whose likelihood values are not less than the likelihood value threshold includes:

[0126] S2021: Project the rough extreme values into a pre-constructed likelihood field, and query the likelihood values corresponding to each of the laser points of the laser data of the current frame, where the likelihood field is constructed according to the laser data of the previous frame;

[0127] S2022: Retrieve the likelihood value threshold, and filter out the outliers whose likelihood values are less than the likelihood value threshold among each of the laser points to obtain a number of matching laser points; where the matching laser points are used as the initial extreme values after screening.

[0128] Further, the laser positioning method is applied to a floor sweeper, which is equipped with a lidar and a gyroscope. The step of collecting laser data includes:

[0129] S101: Collect initial laser data through the lidar and determine whether the floor sweeper tilts during the collection of the initial laser data;

[0130] S102: If the floor sweeper tilts during the collection of the initial laser data, determine whether the environment where the floor sweeper is located belongs to a first preset environment or a second preset environment during the collection of the initial laser data;

[0131] S103: If the environment where the floor sweeper is located belongs to the first preset environment during the collection of the initial laser data, calculate the first laser ranging distance and the second laser ranging distance from the laser emitted by the lidar to the bearing surface through the gyroscope data of the gyroscope. The bearing surface is the surface supporting the floor sweeper;

[0132] S104: Filter the candidate laser data within the preset range of the first laser ranging distance and the second laser ranging distance in the initial laser data to obtain the laser data;

[0133] S105: If the environment where the floor sweeper is located belongs to the second preset environment during the collection of the initial laser data, use the initial laser data as the laser data.

[0134] Further, the step of determining whether the environment where the floor sweeper is located belongs to a first preset environment or a second preset environment during the collection of the initial laser data includes:

[0135] S10201: Retrieve the gyroscope data, which includes multiple gyroscope values;

[0136] S10202: Calculate the standard deviation of the gyroscope data based on each gyroscope value;

[0137] S10203: Determine whether the standard deviation is greater than the standard deviation threshold;

[0138] S10204: If the standard deviation is greater than the standard deviation threshold, determine that the environment where the floor sweeper is located belongs to the first preset environment during the collection of the initial laser data;

[0139] S10205: If the standard deviation is less than the standard deviation threshold, determine that the environment where the floor sweeper is located belongs to the second preset environment during the collection of the initial laser data.

[0140] Further, the step of calculating the first laser ranging distance and the second laser ranging distance from the laser emitted by the lidar to the bearing surface through the gyroscope data of the gyroscope includes:

[0141] S10401: Analyze the first tilt angle and the second tilt angle of the sweeper from the gyroscope data, where the first tilt angle is the current tilt angle of the sweeper on the Y-axis, and the second tilt angle is the current tilt angle of the sweeper around the X-axis;

[0142] S10402: Obtain the height of the lidar from the ground, and calculate the first laser ranging distance based on the height from the ground and the first tilt angle, and calculate the second laser ranging distance based on the height from the ground and the second tilt angle.

[0143] Further, before the step of collecting laser data, it includes:

[0144] S4: Detect whether the sweeper collides during movement through the collision sensor;

[0145] S5: If the sweeper does not collide during movement, obtain the pose increment between two adjacent frames of odometer data from the odometer sequence based on time series, and use the pose increment as the prior pose increment;

[0146] S6: If the sweeper collides during movement, retrieve a preset value and set the preset value as the prior pose increment;

[0147] S7: Retrieve the previous frame pose of the sweeper, and calculate the current frame pose of the sweeper based on the previous frame pose and the prior pose increment.

[0148] An embodiment of the present application further provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, a laser positioning method is implemented. The laser positioning method specifically is:

[0149] S1: Collect laser data, where the laser data includes multiple laser points;

[0150] S2: Find rough extreme values from the laser data through a correlation scan matching method, perform a preset screening operation on the rough extreme values to obtain initial extreme values;

[0151] S3: Use the Gauss-Newton algorithm to optimize the initial extreme values, and converge to obtain global extreme values, where the global extreme values represent the positioning pose.

[0152] Further, the performing a preset screening operation on the rough extreme values to obtain initial extreme values includes:

[0153] S201: Calculate the weighted matching scores for the laser distances corresponding to the rough extreme values respectively, and filter to obtain the initial extreme values with the matching scores within a preset sorting range; and / or,

[0154] S202: Dynamically filter the laser points corresponding to the rough extreme values respectively, and filter from each of the laser points to obtain the initial extreme values with a likelihood value not less than the likelihood value threshold.

[0155] Further, the calculation steps of the matching scores corresponding to a single pose include:

[0156] S2011: Obtain the laser distance of the current laser point;

[0157] S2012: Filter the corresponding weight coefficient from a preset mapping table according to the laser distance, and the preset mapping table includes multiple groups of corresponding laser distance ranges and weight coefficients;

[0158] S2013: Retrieve the laser angle of the current laser point and the grid resolution of the grid map, and substitute the laser distance, the laser angle, the grid resolution, and the weight coefficient into a pre-constructed scoring function for calculation to obtain the matching score.

[0159] Further, the step of dynamically filtering the laser points corresponding to the rough extreme values respectively, and filtering from each of the laser points to obtain the initial extreme values with a likelihood value not less than the likelihood value threshold includes:

[0160] S2021: Project the rough extreme values into a pre-constructed likelihood field, and query the likelihood values corresponding to each of the laser points of the laser data of the current frame, and the likelihood field is constructed according to the laser data of the previous frame;

[0161] S2022: Retrieve the likelihood value threshold, and filter out the outliers with the likelihood value less than the likelihood value threshold among each of the laser points to obtain a number of matching laser points; wherein, the matching laser points are used as the initial extreme values after screening.

[0162] Further, the laser positioning method is applied to a sweeping robot, and the sweeping robot is equipped with a lidar and a gyroscope, and the step of collecting laser data includes:

[0163] S101: Collect initial laser data through the lidar, and judge whether the sweeping robot tilts during the collection of the initial laser data;

[0164] S102: If the floor sweeper tilts during the acquisition of the initial laser data, determine whether the environment where the floor sweeper is located belongs to the first preset environment or the second preset environment during the acquisition of the initial laser data;

[0165] S103: If the environment where the floor sweeper is located belongs to the first preset environment during the acquisition of the initial laser data, calculate the first laser ranging distance and the second laser ranging distance from the laser emitted by the lidar to the bearing surface through the gyroscope data of the gyroscope, where the bearing surface is the surface supporting the floor sweeper;

[0166] S104: Filter the candidate laser data within the preset range of the first laser ranging distance and the second laser ranging distance in the initial laser data to obtain the laser data;

[0167] S105: If the environment where the floor sweeper is located belongs to the second preset environment during the acquisition of the initial laser data, use the initial laser data as the laser data.

[0168] Further, the step of determining whether the environment where the floor sweeper is located belongs to the first preset environment or the second preset environment during the acquisition of the initial laser data includes:

[0169] S10201: Retrieve the gyroscope data, where the gyroscope data includes multiple gyroscope values;

[0170] S10202: Calculate the standard deviation of the gyroscope data based on each of the gyroscope values;

[0171] S10203: Determine whether the standard deviation is greater than the standard deviation threshold;

[0172] S10204: If the standard deviation is greater than the standard deviation threshold, determine that the environment where the floor sweeper is located belongs to the first preset environment during the acquisition of the initial laser data;

[0173] S10205: If the standard deviation is less than the standard deviation threshold, determine that the environment where the floor sweeper is located belongs to the second preset environment during the acquisition of the initial laser data.

[0174] Further, the step of calculating the first laser ranging distance and the second laser ranging distance from the laser emitted by the lidar to the bearing surface through the gyroscope data of the gyroscope includes:

[0175] S10401: Parse the gyroscope data to obtain the first tilt angle and the second tilt angle of the sweeper. The first tilt angle is the tilt angle of the sweeper on the Y-axis currently, and the second tilt angle is the tilt angle of the sweeper around the X-axis currently;

[0176] S10402: Obtain the ground clearance of the lidar, and calculate the first lidar ranging distance based on the ground clearance and the first tilt angle, and calculate the second lidar ranging distance based on the ground clearance and the second tilt angle.

[0177] Further, before the step of collecting laser data, it includes:

[0178] S4: Detect whether the sweeper collides during movement through the collision sensor;

[0179] S5: If the sweeper does not collide during movement, obtain the pose increment between two adjacent frames of odometer data from the odometer sequence based on time sequence, and use the pose increment as the prior pose increment;

[0180] S6: If the sweeper collides during movement, retrieve a preset value and set the preset value as the prior pose increment;

[0181] S7: Retrieve the previous frame pose of the sweeper, and calculate the current frame pose of the sweeper based on the previous frame pose and the prior pose increment.

[0182] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above methods. Among them, any reference to a memory, storage, database, or other medium provided in this application and used in the embodiments can include non-volatile and / or volatile memories. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and Rambus dynamic RAM (RDRAM), etc.

[0183] It should be noted that in this document, the term "including", "comprising", or any other variant thereof is intended to cover non-exclusive inclusion, such that a process, apparatus, first object, or method that includes a series of elements includes not only those elements but also other elements not explicitly listed, or elements that are inherent to such process, apparatus, first object, or method. Without further limitation, an element defined by the statement "including one..." does not exclude the existence of additional identical elements in the process, apparatus, first object, or method that includes the element.

[0184] The above are only the preferred embodiments of this application, and do not limit the patent scope of this application. Any equivalent structure or equivalent process transformation made by using the specification and drawings of this application, or directly or indirectly applied in other related technical fields, shall be equally included in the patent protection scope of this application.

Claims

1. A laser positioning method, characterized in that, Including: Collecting laser data, where the laser data includes multiple laser points; Finding rough extreme values from the laser data of the current frame through a correlation scanning and matching method, and performing a preset screening operation on the rough extreme values to obtain initial extreme values; Using the Gauss-Newton algorithm to optimize the initial extreme values and converging to obtain global extreme values, where the global extreme values represent the positioning pose.

2. The laser positioning method according to claim 1, wherein, The step of performing a preset screening operation on the rough extreme values to obtain initial extreme values includes: Performing weighted calculation of matching scores for the laser distances corresponding to the rough extreme values respectively, and screening to obtain initial extreme values with the matching scores within a preset sorting range; and / or, Dynamically filtering the laser points corresponding to the rough extreme values respectively, and screening from each of the laser points to obtain initial extreme values with a likelihood value not less than a likelihood value threshold.

3. The laser positioning method according to claim 2, characterized in that The calculation steps of the matching score corresponding to a single pose include: Obtaining the laser distance of the current laser point; Filtering the corresponding weight coefficient from a preset mapping table according to the laser distance, where the preset mapping table includes multiple groups of corresponding laser distance ranges and weight coefficients; Retrieving the laser angle of the current laser point and the grid resolution of the grid map, and substituting the laser distance, the laser angle, the grid resolution, and the weight coefficient into a pre-constructed scoring function for calculation to obtain the matching score.

4. The laser positioning method according to claim 2, wherein The step of dynamically filtering the laser points corresponding to the rough extreme values respectively, and screening from each of the laser points to obtain initial extreme values with a likelihood value not less than a likelihood value threshold includes: Projecting the rough extreme values onto a pre-constructed likelihood field, and querying to obtain the likelihood values corresponding to each of the laser points of the laser data of the current frame, where the likelihood field is constructed according to the laser data of the previous frame; Retrieving the likelihood value threshold, and filtering out the outliers with a likelihood value less than the likelihood value threshold among each of the laser points to obtain a number of matching laser points; where the matching laser points are used as the initial extreme values after screening.

5. The laser positioning method according to claim 1, characterized in that, The laser positioning method is applied to a sweeping robot, where the sweeping robot is equipped with a lidar and a gyroscope. The step of collecting laser data includes: Collecting initial laser data through the lidar, and determining whether the sweeping robot tilts during the collection of the initial laser data; If the sweeping robot tilts during the collection of the initial laser data, then determining whether the environment where the sweeping robot is located during the collection of the initial laser data belongs to a first preset environment or a second preset environment; If the environment where the sweeping robot is located during the collection of the initial laser data belongs to the first preset environment, then calculating the first laser ranging distance and the second laser ranging distance from the laser emitted by the lidar to the bearing surface through the gyroscope data of the gyroscope, where the bearing surface is the surface supporting the sweeping robot; Filtering the candidate laser data within the preset range of the first laser ranging distance and the second laser ranging distance in the initial laser data to obtain the laser data; If the environment where the floor sweeper is located belongs to the second preset environment during the process of collecting the initial laser data, then the initial laser data is used as the laser data.

6. The laser positioning method according to claim 5, wherein The step of determining that the environment where the floor sweeper is located belongs to the first preset environment or the second preset environment during the process of collecting the initial laser data includes: Retrieving the gyroscope data, where the gyroscope data includes multiple gyroscope values; Calculating the standard deviation of the gyroscope data based on each of the gyroscope values; Determining whether the standard deviation is greater than the standard deviation threshold; If the standard deviation is greater than the standard deviation threshold, it is determined that the environment where the floor sweeper is located belongs to the first preset environment during the process of collecting the initial laser data; If the standard deviation is less than the standard deviation threshold, it is determined that the environment where the floor sweeper is located belongs to the second preset environment during the process of collecting the initial laser data.

7. The laser positioning method according to claim 5, wherein The step of calculating the first laser ranging distance and the second laser ranging distance from the laser emitted by the lidar to the bearing surface through the gyroscope data of the gyroscope includes: Analyzing the first tilt angle and the second tilt angle of the floor sweeper from the gyroscope data, where the first tilt angle is the current tilt angle of the floor sweeper on the Y-axis, and the second tilt angle is the current tilt angle of the floor sweeper around the X-axis; Obtaining the height of the lidar from the ground, and calculating the first laser ranging distance based on the height from the ground and the first tilt angle, and calculating the second laser ranging distance based on the height from the ground and the second tilt angle.

8. The laser positioning method according to claim 1, wherein Before the step of collecting the laser data, it includes: Detecting whether the floor sweeper collides during the movement through a collision sensor; If the floor sweeper does not collide during the movement, obtaining the pose increment between two adjacent frames of odometer data from the odometer sequence based on time series, and using the pose increment as the prior pose increment; If the floor sweeper collides during the movement, retrieving a preset value and setting the preset value as the prior pose increment; Retrieving the previous frame pose of the floor sweeper, and calculating the current frame pose of the floor sweeper based on the previous frame pose and the prior pose increment.

9. A computer device, comprising a memory and a processor, wherein a computer program is stored in the memory, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the method according to any one of claims 1 to 8.

Images