Robot positioning method and robot

Abstract

Provided in the embodiments of the present application are a robot positioning method and a robot. The robot positioning method comprises: acquiring motion parameters corresponding to a current frame of point cloud which is collected by a robot and motion parameters corresponding to a previous frame of point cloud which is collected by the robot, wherein the motion parameters at least comprise a linear velocity and an angular velocity; at least performing angle filtering, line filtering and voxel filtering processing on the current frame of point cloud, so as to obtain a current frame of optimized point cloud, wherein the angle filtering is used for removing noise points from the current frame of point cloud, the line filtering is used for extracting a structured scene from the current frame of point cloud, and the voxel filtering is used for clustering feature points in the current frame of point cloud; and on the basis of motion parameters corresponding the current frame of optimized point cloud and the motion parameters corresponding to the previous frame of point cloud, calculating a pose corresponding to the robot when same collects the current frame of point cloud, so as to realize positioning. The method can reduce the computation amount of a robot when performing positioning in a complex scenario, thus reducing the computing power requirements for the robot.

Description

Robot positioning method and robot

[0001] This application claims priority to the Chinese patent application filed with the China Patent Office on November 13, 2023, with application number 202311508764.7 and entitled “A Robot Positioning Method and Robot,” the entire contents of which are incorporated by reference into this application. Technical Field

[0002] The present application relates to the field of robots, and more particularly, to a robot positioning method and a robot. Background Art

[0003] Positioning and navigation are key technologies for mobile robots. Reliable positioning based on sensor data is a fundamental and crucial function of mobile robots. Due to the complex application scenarios of robots, in environments such as those with diffuse dust and fog, the point cloud collected by the robot from its surroundings can easily contain a large number of noisy points, resulting in excessive computational overhead during the robot's positioning process. Therefore, reducing the computing power required for robot positioning in complex scenarios is a pressing issue in this field.

[0004] Summary of the Invention

[0005] The present application provides a robot positioning method and a robot, which reduces the amount of calculation required for positioning the robot in complex scenarios and reduces the computing power required by the robot by performing angle filtering, linear filtering, and voxel filtering on the surrounding environment point cloud information collected by the robot.

[0006] In a first aspect, a robot positioning method is provided, comprising obtaining motion parameters corresponding to a current frame point cloud and a previous frame point cloud collected by the robot, wherein the motion parameters include at least linear velocity and angular velocity; performing at least angle filtering, linear filtering, and voxel filtering on the current frame point cloud to obtain an optimized point cloud of the current frame; wherein angle filtering is used to remove noise points in the current frame point cloud, linear filtering is used to extract structured scenes from the current frame point cloud, and voxel filtering is used to cluster feature points in the current frame point cloud; and according to the motion parameters corresponding to the current frame optimized point cloud and the previous frame point cloud, calculating the corresponding posture of the robot when collecting the current frame point cloud to achieve positioning.

[0007] In an embodiment of the present application, the robot performs angle filtering, line filtering, and voxel filtering on the point cloud, and realizes positioning based on the processed optimized point cloud, thereby reducing the amount of calculation required for positioning in complex scenes such as dust and fog diffusion, and reducing the computing power requirements of the robot.

[0008] In combination with the first aspect, in some implementations of the first aspect, filtering is performed in the order of angle filtering, linear filtering, and voxel filtering, that is, linear filtering is performed on the point cloud after angle filtering, and voxel filtering is performed on the point cloud after linear filtering.

[0009] In the embodiments provided herein, linear filtering is performed after angular filtering, resulting in less noise in the feature points that need to be processed by linear filtering, which can increase the accuracy of the structured scene extracted by linear filtering. Voxel filtering is performed after linear filtering, making the voxel filtering results more accurate. At the same time, since the number of feature points that need to be processed by voxel filtering is reduced, the amount of computation required for voxel filtering can also be reduced. Performing filtering in the order of angular filtering, linear filtering, and voxel filtering can remove a large number of noise points in the point cloud, which is particularly suitable for robot positioning scenarios with low computing power.

[0010] In conjunction with the first aspect, in certain implementations of the first aspect, the robot performs downsampling before filtering the point cloud. The collected point cloud data can be periodically downsampled by setting a certain time interval to achieve a consistent density of the point cloud data and reduce the computational complexity of subsequent robot positioning.

[0011] In combination with the first aspect, in certain implementations of the first aspect, angle filtering includes: reading a preset parameter configuration table to obtain relevant parameters, the relevant parameters including but not limited to: minimum filtering angle, maximum filtering angle; calculating the angle between the current feature point of the current frame point cloud and the feature point in the previous frame point cloud that matches the current feature point, if the angle is less than the minimum filtering angle, or the angle is greater than the maximum filtering angle, then the corresponding feature point is removed from the current frame point cloud.

[0012] In the embodiments provided herein, angle filtering is used to address the issue of excessive noise in point cloud information when the robot is in a particular environment. For example, when the robot is surrounded by dust or fog, the dust or fog creates a diffusion effect, resulting in a chaotic, noisy point cloud in the laser point cloud or image point cloud collected by the robot. Angle filtering of this point cloud can generate point cloud data for the region of interest, thereby reducing the computational complexity of the robot's positioning while improving positioning accuracy.

[0013] In combination with the first aspect and certain implementation methods of the first aspect, in other implementation methods of the first aspect, the straight line filtering includes: selecting the current frame point cloud and some points in the previous frame point cloud to construct a straight line and calculating the straight line equation of the current straight line based on the coordinates of the some points; adding the remaining points in the point cloud that are not used to construct the straight line to the current straight line, and calculating the distance from the current added point to the current straight line based on the coordinates of the current added point and the straight line equation of the current straight line; if the distance from the current added point to the current straight line is greater than a preset distance threshold, the current added point is eliminated and the next point is calculated; conversely, if the distance from the current added point to the current straight line is within the preset distance threshold range, the straight line equation of the current straight line is recalculated through straight line fitting.

[0014] In combination with the first aspect and certain implementations of the first aspect, in other implementations of the first aspect, the straight line filtering also includes: if the distances between all currently added points and the current straight line are outside the preset distance threshold range, then some points are reselected in the current frame point cloud and the previous frame point cloud to reconstruct the straight line.

[0015] In an embodiment of the present application, due to the influence of noise, structured scenes such as straight lines in the robot's surrounding environment may not appear as regular straight lines in the point cloud collected by the robot. It is necessary to perform straight line filtering on the point cloud data to extract structured scenes such as straight lines, so as to facilitate subsequent further perception of the robot's surrounding environment.

[0016] For example, the straight line structure in the robot's surrounding environment may be the boundary of the robot's moving path, or a marker on the robot's moving path, etc., which is not specifically limited in this application.

[0017] In combination with the first aspect and certain implementations of the first aspect, in other implementations of the first aspect, voxel filtering includes: creating a voxel grid for the current frame point cloud, and replacing the feature points contained in each voxel grid with the centroid or center of the feature points contained in each voxel grid.

[0018] In the embodiment of the present application, voxel filtering can not only effectively cluster the point cloud without destroying the structure of the point cloud itself, but also further reduce the amount of point cloud information, thereby reducing the amount of computational complexity of the robot's subsequent processing of the point cloud information and reducing the computing power requirements of the robot.

[0019] In combination with the first aspect and certain implementation methods of the first aspect, in other implementation methods of the first aspect, the corresponding posture of the robot when collecting the current frame point cloud is calculated based on the motion parameters corresponding to the current frame optimized point cloud and the previous frame point cloud, including: matching the current frame optimized point cloud with the preset map to obtain the initial posture corresponding to the current frame point cloud; the preset map is obtained by splicing the historical frame point clouds obtained by scanning the scene; based on the initial posture and motion parameters corresponding to the previous frame point cloud, the predicted posture corresponding to the current frame point cloud is obtained; and the initial posture is corrected using the predicted posture of the current frame point cloud to obtain the posture of the robot when collecting the current frame point cloud.

[0020] In an embodiment of the present application, the robot posture is obtained by correcting the initial posture through predicted posture, which can improve the problem of inaccurate posture results caused by measurement errors in the initial posture determined only by point cloud information, thereby improving the accuracy of robot positioning.

[0021] In combination with the first aspect and certain implementation methods of the first aspect, in other implementation methods of the first aspect, based on the initial pose and motion parameters corresponding to the previous frame point cloud, the predicted pose corresponding to the current frame point cloud is obtained, including: using the initial pose, motion parameters and preset motion model of the previous frame point cloud to predict the pose corresponding to the current frame point cloud, and obtaining the predicted pose corresponding to the robot when collecting the current frame point cloud; the preset motion model is a two-wheel differential model, and the motion parameters include the linear velocity of the left and right wheels of the robot when collecting the previous frame point cloud, which includes: calculating the angular velocity of the left and right wheel centers when collecting the previous frame point cloud based on the linear velocity of the left and right wheels when collecting the previous frame point cloud; obtaining the time interval between the robot collecting the previous frame point cloud and the current frame point cloud, and using the linear velocity and angular velocity and time interval corresponding to the robot collecting the previous frame point cloud to calculate the predicted pose of the robot when collecting the current frame point cloud.

[0022] In combination with the first aspect and certain implementation methods of the first aspect, in other implementation methods of the first aspect, the initial pose is corrected using the predicted pose of the current frame point cloud to obtain the pose of the robot when collecting the current frame point cloud, including: obtaining the current system noise of the robot, using the predicted pose of the current frame point cloud and the system noise to perform nonlinear modeling to obtain an estimated pose corresponding to the initial pose of the current frame point cloud; calculating the difference between the initial pose and the estimated pose and combining the Kalman gain to correct the predicted pose to obtain the pose of the robot when collecting the current frame point cloud.

[0023] In the embodiment of the present application, the robot takes system noise into account when calculating the posture, which is more in line with the actual situation of the robot during movement, making the robot's calculation results of the posture more accurate.

[0024] In the embodiment of the present application, by combining the Kalman gain and the motion model, the problem of non-Gaussian linear noise introduced by the robot's odometry slip failure can be effectively solved. Compared with algorithms such as the Extended Kalman Filter (EKF) and the Unscented Kalman Filter (UKF), the Kalman gain method used by the robot in the embodiment of the present application only needs to calculate the variance matrix to obtain the Kalman gain, and there is no need to solve the Jacobian matrix. It is not only simpler to implement, but also reduces resource overhead, and can be integrated on an embedded platform with limited computing power in the robot.

[0025] In the second aspect, a robot is provided, comprising: a point cloud acquisition device for acquiring point cloud data of the robot's surrounding environment; a motion acquisition device for acquiring motion parameters of the robot, the motion parameters including linear velocity and / or angular velocity; and a main control chip for processing the point cloud data acquired by the point cloud acquisition device and the motion parameters acquired by the motion acquisition device according to the robot positioning method in the first aspect to obtain positioning information of the robot.

[0026] In a third aspect, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed, the method in the first aspect is implemented. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] FIG1 is a schematic diagram of a robot application scenario according to an embodiment of the present application.

[0028] FIG2 is a flow chart of a robot positioning method according to an embodiment of the present application.

[0029] FIG3 is a flowchart of angle filtering according to an embodiment of the present application.

[0030] FIG4 is a flow chart of linear filtering according to an embodiment of the present application.

[0031] FIG5 is a flowchart of the robot posture calculation according to an embodiment of the present application

[0032] FIG6 is a schematic diagram of a motion model according to an embodiment of the present application.

[0033] FIG7 is a schematic diagram of a robot according to an embodiment of the present application.

[0034] FIG8 is a schematic diagram of a robot positioning device according to an embodiment of the present application. DETAILED DESCRIPTION

[0035] The technical solutions in the embodiments of the present application will be described below in conjunction with the accompanying drawings in the embodiments of the present application. In the description of the embodiments of the present application, unless otherwise specified, " / " means or, for example, A / B can mean A or B; "and / or" in this article is merely a description of the association relationship of associated objects, indicating that three relationships can exist, for example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone. In addition, in the description of the embodiments of the present application, "multiple" means two or more than two.

[0036] In the following, the terms "first" and "second" are used for descriptive purposes only and should not be understood to indicate or imply relative importance or implicitly specify the quantity of the technical features indicated. Therefore, a feature specified as "first" or "second" may explicitly or implicitly include one or more of the features.

[0037] Positioning and navigation are key technologies for mobile robots. Positioning involves matching sensor data with a current prior map to determine the robot's position within that map. Reliable positioning through sensor data is a fundamental and crucial function of mobile robots. Considering the performance limitations of individual sensors and the influence of environmental factors, heterogeneous sensor data fusion is currently the most popular approach for robot positioning. Commonly used sensors in robots include odometry, light detection and ranging (LiDAR), and image acquisition modules.

[0038] An odometry is a device that uses data from mobile sensors to estimate the change in an object's position over time. It is often used in robotic systems to estimate the distance the robot has moved relative to its initial position. Commonly used odometry methods include wheel-mounted odometry and inertial odometry. Wheel-mounted odometry uses wheel speed sensors to measure the linear and angular velocities of the robot's wheels to estimate the distance and direction the robot has moved on the ground. Inertial odometry uses information such as acceleration and angular velocity provided by inertial sensors to estimate the evolution of the robot's position over time.

[0039] LiDAR scans the external environment by emitting light pulses to construct a point cloud of the external environment. LiDAR may include a laser source and a laser detector. The laser source is used to generate laser pulses and transmit them toward an object, while the laser detector receives the laser pulses reflected back by the object. Using the time difference between the transmission and reception of the laser pulses, the laser detector determines the depth of the object and the LiDAR, thereby constructing a point cloud of the surrounding environment. A point cloud is a data format that describes the distribution of data points in three-dimensional space. These data points can simulate and / or represent the external scene's shape and spatial characteristics. The number of data points in a point cloud is often large. For example, when generating a high-resolution image of an external scene, the point cloud may contain millions of data points.

[0040] The image acquisition module is mainly used for local images of the external environment to assist in positioning. In one embodiment, the image acquisition module can include any one or more sets of cameras such as a depth camera, a color camera, an infrared camera, and a grayscale camera.

[0041] FIG1 is a schematic diagram of a robot application scenario provided in an embodiment of the present application.

[0042] In the robot application scenario shown in FIG1 , the robot 100 collects point cloud information of the surrounding environment and motion parameter information of the robot during movement, and determines the posture of the robot in the current frame by combining the point cloud information and the motion parameter information.

[0043] Exemplarily, the robot's posture may include the robot's position and posture in the map. Any rigid body can be accurately and uniquely represented by its position and posture in the spatial coordinate system (OXYZ). Taking the robot moving in a two-dimensional plane as an example, the robot's posture can be expressed as: W = (X, Y, θ). Among them, X represents the coordinate of the robot along the X-axis in the coordinate system of the robot's motion, and Y represents the coordinate of the robot along the Y-axis in the coordinate system of the robot's motion. θ represents the angle between the direction vector of the robot's direction and the coordinate axis in the coordinate system of the robot's motion.

[0044] Due to the complexity of robot application scenarios, in application scenarios such as dust and fog diffusion, the point cloud of the surrounding environment collected by the robot is likely to contain a large number of noise points, resulting in excessive computational complexity during the robot positioning process.

[0045] FIG2 is a flow chart of a robot positioning method 200 according to an embodiment of the present application.

[0046] As shown in Figure 2, the robot positioning method performs angle filtering, line filtering, and voxel filtering on the point cloud, and realizes positioning based on the processed optimized point cloud. The specific steps are as follows:

[0047] S210, obtaining motion parameters: obtaining motion parameters corresponding to the current frame point cloud and the previous frame point cloud collected by the robot, wherein the motion parameters include at least linear velocity and angular velocity.

[0048] S220, filtering processing: performing at least angle filtering, linear filtering and voxel filtering on the current frame point cloud to obtain the current frame optimized point cloud; wherein, angle filtering is used to remove noise points in the current frame point cloud, linear filtering is used to extract structured scenes from the current frame point cloud, and voxel filtering is used to cluster feature points in the current frame point cloud.

[0049] Optionally, due to the different densities of the collected laser point cloud, the robot can also perform downsampling (downsampling) processing on the current frame point cloud. It can periodically downsample the collected point cloud data by setting a certain time interval to reduce the amount of calculation. Downsampling methods include but are not limited to: random sampling, average sampling, and nearest neighbor sampling; among them, random sampling selects a portion of points in the point cloud as the sampling result, uniform sampling divides the point cloud and selects a point in each divided area as the sampling result, and nearest neighbor sampling calculates the distance between each point and the surrounding points and selects the point with the largest or smallest distance as the sampling result. In order to make the density of the point cloud data consistent, the point cloud can be periodically downsampled.

[0050] S230, pose calculation: Based on the motion parameters corresponding to the current frame optimized point cloud and the previous frame point cloud, the pose corresponding to the robot when collecting the current frame point cloud is calculated to achieve positioning.

[0051] In the embodiment of the present application, the robot obtains multiple frames of local point clouds collected by the laser radar and the motion parameters collected synchronously by the odometer when collecting multiple frames of local point clouds. The current frame point cloud in the multiple frames of local point clouds is filtered to eliminate noise caused by dust and fog in the collected point cloud data, thereby obtaining a filtered current frame point cloud. By performing angle filtering, linear filtering, and voxel filtering on the point cloud, and achieving positioning based on the processed optimized point cloud, the robot reduces the amount of calculation required for positioning in complex scenes such as dust and fog diffusion, thereby reducing the computing power required by the robot. At the same time, because the robot positioning method in the embodiment of the present application can be used in general scenes and special scenes such as dust and fog, it has better versatility.

[0052] In an embodiment of the present application, the robot eliminates noise points in the current frame point cloud through angle filtering, extracts structured scenes from the current frame point cloud through linear filtering, and clusters feature points in the current frame point cloud through voxel filtering. This allows the robot to reduce the noise of the collected point cloud information in application scenarios such as dust and fog diffusion, reduce the amount of calculation when the robot processes point cloud information, and improve the robot's positioning accuracy.

[0053] In a possible implementation, filtering is performed in the order of angle filtering, line filtering, and voxel filtering. That is, line filtering is performed on the point cloud after angle filtering, and voxel filtering is performed on the point cloud after line filtering.

[0054] In the embodiments provided herein, linear filtering is performed after angular filtering, resulting in less noise in the feature points that need to be processed by linear filtering, which can increase the accuracy of the structured scene extracted by linear filtering. Voxel filtering is performed after linear filtering, making the voxel filtering results more accurate. At the same time, since the number of feature points that need to be processed by voxel filtering is reduced, the amount of computation required for voxel filtering can also be reduced. Performing filtering in the order of angular filtering, linear filtering, and voxel filtering can remove a large number of noise points in the point cloud, which is particularly suitable for robot positioning scenarios with low computing power.

[0055] FIG3 is a flowchart of angle filtering S221 according to an embodiment of the present application.

[0056] As shown in FIG3 , as a possible implementation method, the robot performs angle filtering on the downsampled point cloud data in the order of parameter acquisition and angle calculation.

[0057] S2211, parameter acquisition: read the preset parameter configuration table and obtain relevant parameters, including but not limited to: minimum filtering angle and maximum filtering angle.

[0058] S2212, angle calculation: calculate the angle between the current feature point of the current frame point cloud and the feature point in the previous frame point cloud that matches the current feature point. If the angle is less than the minimum filtering angle or greater than the maximum filtering angle, the corresponding feature point is removed from the current frame point cloud.

[0059] Specifically, the robot obtains the coordinates of a point in the previous frame point cloud, namely the first feature point, and a point in the current frame point cloud corresponding to the feature point, namely the second feature point.

[0060] Furthermore, the robot takes the origin of the point cloud's overall coordinate system as a common starting point and constructs vectors passing through the first feature point and the second feature point respectively; based on the constructed vectors, the robot calculates the corresponding points in the previous and next two frames of point clouds, that is, the angle between the first feature point and the second feature point.

[0061] It should be understood that the coordinates of point clouds of different frames are expressed in the same coordinate system, that is, the point cloud of the previous frame and the point cloud of the current frame are expressed using the same coordinate system.

[0062] In one possible implementation, it is assumed that the vector formed by the first feature point and the origin is The vector formed by the second characteristic point and the common origin is That is, the angle θ between the corresponding points in the point clouds of the previous and next frames can be calculated using the following formula:

[0063] or

[0064] Furthermore, the robot determines whether to remove the point involved in the calculation in the current frame point cloud based on the calculated angle value result.

[0065] Specifically, if the calculated angle θ is greater than or equal to the minimum filter angle preset in the parameter configuration table, and less than or equal to the maximum filter angle preset in the parameter configuration table, then the point in the current frame point cloud participating in the calculation is retained. Conversely, if the calculated angle θ is less than the minimum filter angle preset in the parameter configuration table, or greater than the maximum filter angle preset in the parameter configuration table, then the point in the current frame point cloud participating in the calculation is removed.

[0066] In the embodiments provided herein, angle filtering is used to address the issue of excessive noise in point cloud information when the robot is in a particular environment. For example, when the robot is surrounded by dust or fog, the dust or fog creates a diffusion effect, resulting in a chaotic, noisy point cloud in the laser point cloud or image point cloud collected by the robot. Angle filtering of this point cloud can generate point cloud data for the region of interest, thereby reducing the computational complexity of the robot's positioning while improving positioning accuracy.

[0067] FIG4 is a flow chart of the linear filtering S222 according to an embodiment of the present application.

[0068] As shown in FIG4 , as a possible implementation method, the robot performs straight line filtering on the point cloud information in the order of straight line construction, feature point addition, distance judgment, and straight line reconstruction.

[0069] S2221, straight line construction: select the current frame point cloud and some points in the previous frame point cloud to construct a straight line and calculate the straight line equation of the current straight line based on the coordinates of the some points.

[0070] S2222, feature point addition: add the remaining points in the point cloud that are not used to construct the line to the current line, and calculate the distance from the current point to the current line based on the coordinates of the current point and the line equation of the current line.

[0071] S2223, distance judgment: If the distance from the current added point to the current straight line is greater than the preset distance threshold, the current added point is eliminated and the next point is calculated; conversely, if the distance from the current added point to the current straight line is within the preset distance threshold, the linear equation of the current straight line is recalculated through linear fitting.

[0072] Furthermore, the robot takes the refitted straight line as the current straight line and continues to add the remaining points until all points are traversed.

[0073] As a possible implementation, the linear filtering S222 further includes:

[0074] S2224, straight line reconstruction: If the distances between all currently added points and the current straight line are outside the preset distance threshold range, some points are reselected in the current frame point cloud and the previous frame point cloud to reconstruct the straight line.

[0075] Optionally, before performing S2221 line construction, the point cloud sets that have been conditionally filtered are sequentially placed into a preset queue, wherein the preset queue is a continuous storage space with a first-in-first-out reading rule.

[0076] Optionally, when performing S2221 straight line construction, some points may be read from the two point clouds at the head of the preset queue to construct the straight line.

[0077] In an embodiment of the present application, due to the influence of noise, structured scenes such as straight lines in the robot's surrounding environment may not appear as regular straight lines in the point cloud collected by the robot. It is necessary to perform straight line filtering on the point cloud data to extract structured scenes such as straight lines, so as to facilitate subsequent further perception of the robot's surrounding environment.

[0078] In the embodiment of the present application, the preset queue ensures that the point cloud is calculated in sequence, which can ensure that the robot processes two adjacent frames of point cloud without repeated calculations, thereby improving the calculation efficiency of the linear filtering.

[0079] For example, the straight line structure in the robot's surrounding environment may be the boundary of the robot's moving path, or a marker on the robot's moving path, etc., which is not specifically limited in this application.

[0080] In a possible implementation, voxel filtering creates a voxel grid for the current frame point cloud, and replaces the feature points contained in each voxel grid with the centroid or center of the feature points contained in each voxel grid.

[0081] In an embodiment of the present application, all other points in each voxel in the point cloud information are represented by one point, so that voxel filtering can not only effectively cluster the point cloud without destroying the structure of the point cloud itself, but also further reduce the amount of point cloud information, thereby reducing the amount of calculation required for the robot to subsequently process the point cloud information, and reducing the computing power requirements of the robot.

[0082] FIG5 is a flowchart of robot posture calculation S230 according to an embodiment of the present application.

[0083] Please refer to FIG5 . In one possible implementation, when the robot calculates the posture, it performs the following steps in the order of initial posture acquisition, predicted posture acquisition, and posture correction.

[0084] S231, initial pose acquisition: Match the current frame optimized point cloud with the preset map to obtain the initial pose corresponding to the current frame point cloud; the preset map is obtained by splicing the historical frame point clouds obtained by scanning the scene.

[0085] Specifically, the robot matches the filtered current frame point cloud with a map generated based on point clouds at historical moments to obtain the initial posture of the robot when collecting the current frame point cloud; wherein, the map is obtained by scanning the scene to obtain multiple frames of local point clouds, and the multiple frames of local point clouds are spliced ​​together.

[0086] Exemplarily, before collecting multiple frames of local point clouds and motion parameters, that is, before the robot executes S210, the robot can pre-scan the target scene to obtain multiple frames of local point clouds, and splice the pre-scanned multiple frames of local point clouds to obtain a map of the target scene, that is, the robot pre-builds the map.

[0087] It should be understood that since the map is pre-constructed, the constructed map can be understood as a local map generated based on the point cloud of historical moments; wherein the historical moments may include all moments corresponding to the processed map, or the previous moment corresponding to the current frame.

[0088] In one possible implementation, the robot uses an iterative closest point (ICP) algorithm to match the filtered point cloud of the current frame with a map generated based on point clouds at historical moments to obtain the initial pose of the current frame.

[0089] Specifically, the robot can use the point cloud in the map generated based on the historical point cloud as the reference point cloud Q = {q1,q2,…,q n}, the point cloud after filtering of the current frame is the source point cloud P = {p1,p2,…,p n The robot transforms the source point cloud P through a series of rotation matrices R and translation matrices T to make the source point cloud P as close as possible to the reference point cloud Q.

[0090] In one possible implementation, the robot can construct an optimization function based on the least squares method to calculate the rotation matrix R and the translation matrix T. The optimization function formula is:

[0091] Among them, ω iRepresents the weight of each feature point in the point cloud, and the value of the weight can be proportional to the confidence of the corresponding feature point. For example, the confidence can be determined based on the laser reflection intensity of the laser point cloud information or the estimated accuracy of the feature points of the image point cloud data. When the value calculated by the above optimization function is the smallest, it means that R and T are optimal values. When R and T are optimal values, the current frame point cloud and the map generated based on the historical point cloud have the best matching effect. Based on the matching result of the current frame point cloud and the map generated based on the historical point cloud, the position information of the current frame point cloud relative to the map can be obtained, which is regarded as the initial posture of the robot at the current moment, that is, the initial positioning information of the robot at the current moment.

[0092] In an embodiment of the present application, each feature point in the point cloud after filtering of the current frame corresponds to a different weight in the optimization function formula according to its confidence level, so that the feature points that are more likely to correspond to the actual scene around the robot are more important in the calculation, and the feature points that are more likely to be noise are less important in the calculation, thereby making the calculation results of the optimization function more accurate.

[0093] S232, predicted pose acquisition: based on the initial pose and motion parameters corresponding to the previous frame point cloud, the predicted pose corresponding to the current frame point cloud is acquired.

[0094] In one possible implementation, the robot can also obtain the predicted pose corresponding to the current frame point cloud based on the predicted pose or optimized pose corresponding to the previous frame point cloud and motion parameters. Exemplarily, the motion parameters may include information such as integrated angular velocity and linear velocity, which are not specifically limited in this application.

[0095] S233, posture correction: correcting the initial posture using the predicted posture of the current frame point cloud to obtain the posture of the robot when collecting the current frame point cloud.

[0096] In an embodiment of the present application, the robot posture is obtained by correcting the initial posture through predicted posture, which can improve the problem of inaccurate posture results caused by measurement errors in the initial posture determined only by point cloud information, thereby improving the accuracy of robot positioning.

[0097] In one possible implementation, when the robot obtains the predicted pose corresponding to the current frame point cloud based on the initial pose and motion parameters corresponding to the previous frame point cloud, it uses the initial pose, motion parameters and preset motion model of the previous frame point cloud to predict the pose corresponding to the current frame point cloud, and obtains the predicted pose corresponding to when the robot collects the current frame point cloud.

[0098] In a possible implementation, the preset motion model is a two-wheel differential model, and the motion parameters include the linear speeds of the left and right wheels of the robot when collecting the previous frame of point cloud.

[0099] Specifically, the angular velocity of the left and right wheel centers when collecting the previous frame of point cloud is calculated based on the linear velocity of the left and right wheels when collecting the previous frame of point cloud; the time interval between the robot collecting the previous frame of point cloud and the current frame of point cloud is obtained, and the linear velocity, angular velocity and time interval corresponding to the robot collecting the previous frame of point cloud are used to calculate the predicted posture of the robot when collecting the current frame of point cloud.

[0100] FIG6 is a schematic diagram of a motion model according to an embodiment of the present application.

[0101] As shown in FIG6 , in a possible implementation, a motion model is preset in the robot. The robot is a two-wheeled robot. The distance between the two wheels is l. The preset motion model is a two-wheel differential model.

[0102] Specifically, during the robot's motion, the robot's motion trajectory is differentiated into multiple straight lines according to time. Assuming that there is no noise influence during the robot's motion, in each straight line of the robot's motion trajectory differentiated by time, the linear velocities of the left and right wheels of the robot when the current frame point cloud is collected at the current moment are v and v respectively. l and v r , then the centerline speed of the two wheels is: v k =(v l +v r ) / 2

[0103] Based on v k =ω k r, where r is the distance from the center O of the robot's circular motion trajectory to the center points of the two wheels, then the angular velocity is: ω k =(v r -v l ) / l

[0104] The coordinates of the center positions of the two wheels of the robot are (x k ,y k ), the angle between the direction vector of the robot and the X-axis in the coordinate system of the robot's motion is θ k , that is, the robot's current frame pose is: [x k y k θ k ]. Assuming that the time interval between the current frame and the previous moment is Δt, the predicted position of the robot in the current frame satisfies the following formula: k =x k-1 +v k-1 cosθ k-1 Δt y k =y k-1 +v k-1 sinθ k-1 Δt θ k =θ k-1 +ω k-1 Δt

[0105] It should be understood that assuming that there is no noise influence during the movement of the robot is an ideal situation. The movement of the robot is generally accompanied by noise.

[0106] In another possible implementation, it is assumed that noise exists during the movement of the robot, and thus the preset motion model is a motion model with noise.

[0107] Please continue to refer to Figure 6. Assume that the coordinates of the center point O of the robot's motion trajectory are (X o ,Y o ), the initial posture is [X, Y, θ]. According to the geometric relationship of the robot's motion trajectory, the coordinates of the center point O of the robot's motion trajectory can be expressed as: X O =X-rcos(θ-90°)=X-rsinθ Y O =Y-rsin(θ-90°)=Y-rcosθ

[0108] Furthermore, suppose that the robot's motion parameters at time t are measured values ​​of linear velocity and angular velocity v t and ω t , based on the relationship between linear velocity and angular velocity r = v t / ω t , the coordinates of the center point O of the robot's motion trajectory can be transformed into:

[0109] After the robot moves for a time period of Δt, the robot moves v in the direction of motion. t Δt and rotate ω t Δt, based on the above formula, the predicted position of the robot in the current frame [X′, Y′, θ′] can be obtained as: θ′=θ+ω t Δt

[0110] The pose of the associated robot before and after the movement, that is, the predicted pose of the robot moving from the initial pose to the current frame after Δt time, is expressed as: θ′=θ+ω t Δt

[0111] It should be understood that in the above process of calculating the predicted pose of the current frame based on the initial pose, the linear velocity v used is t and angular velocity ω t is the ideal value. During the actual movement of the robot, the robot will be affected by noise and cause errors in the motion parameters.

[0112] In another possible implementation, when the robot calculates the predicted pose of the current frame based on the initial pose, it adds an error Δv caused by noise that follows a Gaussian distribution. t , Δω t and Δθ, and the linear velocity and angular velocity corresponding to the current frame t are expressed as: θ′=θ+ω t Δt+Δθ

[0113] Furthermore, the predicted pose of the robot in the current frame can be expressed as: θ′=θ+ω t Δt+Δθ

[0114] In one possible implementation, the robot uses the predicted pose of the current frame point cloud to correct the initial pose, and obtaining the estimated pose of the robot when collecting the current frame point cloud includes: obtaining the current system noise of the robot, using the predicted pose of the current frame point cloud and the system noise to perform nonlinear modeling, and obtaining the estimated pose corresponding to the initial pose of the current frame point cloud.

[0115] Furthermore, the difference between the initial pose and the estimated pose is calculated and combined with the Kalman gain to correct the predicted pose to obtain the pose of the robot when collecting the current frame point cloud.

[0116] Specifically, the robot can Determine the predicted pose when collecting the current frame point cloud at the current moment. is the initial pose in the previous frame of the current frame, u k-1 It includes the motion parameters in the previous frame of the current frame, and f() is the motion model expression function that does not include noise.

[0117] In another possible implementation, the robot uses the formula Determine the predicted pose when collecting the current frame point cloud at the current moment. is the initial pose in the previous frame of the current frame, u k-1 Including the motion parameters in the previous frame, is the measured noise in the frame before the current frame, and f() is the motion model expression function including noise.

[0118] In the embodiment of the present application, the robot takes measurement noise into account when calculating the posture, which is more in line with the actual situation of the robot during movement, making the robot's calculation results of the posture more accurate.

[0119] Furthermore, the robot combines the predicted pose value when collecting the current frame point cloud at the current moment and system noise Perform nonlinear modeling to obtain the estimated pose value corresponding to the observed pose value when the current frame point cloud is collected at the current moment, taking into account the system noise in, g() represents the modeling function.

[0120] Furthermore, the robot calculates the initial pose y k and estimated pose The difference, combined with the Kalman gain K k Predicted pose value Correction is performed to obtain the robot's position when collecting the current frame point cloud. The specific method is:

[0121] in, is the predicted pose of the current frame, K k is the Kalman gain, y k is the initial pose of the current frame, is the estimated pose of the current frame.

[0122] In one possible implementation, the Kalman gain K k The initial pose y k Autocovariance matrix and predicted pose of With the initial pose y k The cross-covariance matrix of is calculated.

[0123] In the embodiment of the present application, by combining the Kalman gain and the motion model, the problem of non-Gaussian linear noise introduced by the robot's odometry slip failure can be effectively solved. Compared with algorithms such as the Extended Kalman Filter (EKF) and the Unscented Kalman Filter (UKF), the Kalman gain method used by the robot in the embodiment of the present application only needs to calculate the variance matrix to obtain the Kalman gain, and there is no need to solve the Jacobian matrix. It is not only simpler to implement, but also reduces resource overhead, and can be integrated on an embedded platform with limited computing power in the robot.

[0124] In one possible implementation, the robot time-aligns the collected point cloud information with the motion parameters. Methods for time-alignment include, but are not limited to, time-stamping the point cloud information and motion parameters, synchronizing the sensors that collect the point cloud information and motion parameters through the same processor, and adjusting the system time.

[0125] In an embodiment of the present application, the use of a time alignment method can ensure that the sensors that collect various data in the robot are located in the same time system, which is used to ensure the synchronization of various types of data collected by the robot, thereby ensuring the correct correspondence between different data in subsequent calculations and improving the positioning accuracy.

[0126] It should be understood that the above is an embodiment of obtaining positioning information based on point cloud data obtained by the robot, but in actual situations, the robot can also obtain data obtained by multiple sensors, such as image data collected by an image acquisition module or motion parameters recorded by an odometer. In order to further improve the positioning accuracy of the robot, after the robot obtains the posture of the current frame, the calculated posture can be compared with the posture obtained based on point cloud information, the posture obtained based on image information, and the posture obtained based on motion parameters. If the difference is greater than a preset range, the posture compared with the posture calculated for the current frame of the robot is corrected, otherwise no correction is required, thereby realizing the fusion positioning of the robot's multi-sensor data; among them, the posture obtained based on point cloud information can be obtained by comparing the point clouds of the previous and next two frames; the posture obtained based on image information can be obtained by comparing the images of the previous and next two frames; the posture obtained based on motion parameters can be obtained by measurement.

[0127] FIG7 is a schematic diagram of a robot 100 according to an embodiment of the present application.

[0128] In one possible implementation, the robot 100 includes a point cloud acquisition device for collecting point cloud data of the robot's surrounding environment; a motion acquisition device for synchronously collecting the robot's motion parameters while the point cloud acquisition device collects point cloud data, the motion parameters including linear velocity and / or angular velocity; and a main control chip for processing the point cloud data collected by the point cloud acquisition device and the motion parameters collected by the motion acquisition device according to the robot positioning method in the embodiment of the present application to obtain the robot's positioning information.

[0129] Specifically, as shown in FIG7 , the robot 100 may include a laser radar 110, an image acquisition module 120, and a motion acquisition module 130. The laser radar 110, the image acquisition module 120, and the motion acquisition module 130 are installed on the body of the robot 100. The laser radar 110 is used to scan the application scene at the current moment to obtain a multi-frame point cloud corresponding to the current moment. The image acquisition module 120 is used to collect multi-frame images of the application scene while the laser radar is working. The motion acquisition module 130 is used to record the motion parameters of the robot body, i.e., motion parameters, when the laser radar 110 and the image acquisition module 120 are collecting data in the current frame. The motion parameters may include linear velocity and / or angular velocity, etc.

[0130] Furthermore, the main control chip is embedded in the robot body, and is used to process multiple frames of point clouds, multiple frames of images and corresponding motion parameters recorded each time data is collected according to the robot positioning method in the embodiment of the present application, so as to obtain the positioning information of the robot body in the application scenario at the current moment.

[0131] Figure 8 shows a schematic diagram of a robot positioning device provided by the present application. The robot positioning device shown in Figure 8 includes a processor 810, which is used to implement any of the methods in the above embodiments.

[0132] The processor 810 mentioned in the embodiments of the present application may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field programmable gate arrays (FPGA), neural network chips, or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor. It should be understood that when the processor is a neural network chip, the device may not include a memory.

[0133] In one embodiment, the apparatus further includes a memory 820 , which can store information such as point clouds collected in the above embodiments and computer program products required to execute the methods in the above embodiments.

[0134] The memory 820 and the processor 810 may be coupled via a bus. The memory 820 is used to store computer program instructions or data. The processor 810 reads the computer instructions stored in the memory 820 or reads the data stored in the memory 820 to execute the method in the above embodiment.

[0135] The processor 810 and the memory 820 may be placed separately or integrated. For example, the processor 810 may be a processing device in a user's mobile terminal with independent data processing capabilities, a processing device in a collection device, or other processing devices.

[0136] It should also be understood that the memory 820 mentioned in the embodiments of the present application may be a volatile memory and / or a non-volatile memory. Among them, the non-volatile memory may be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), or a flash memory. The volatile memory may be a random access memory (RAM). For example, RAM can be used as an external cache. By way of example and not limitation, RAM includes the following forms: static random access memory (SRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDR SDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link dynamic random access memory (SLDRAM), and direct rambus RAM (DR RAM).

[0137] It should be noted that when the processor is a general-purpose processor, DSP, ASIC, FPGA or other programmable logic device, discrete gate or transistor logic device, discrete hardware component, the memory (storage module) can be integrated into the processor.

[0138] An embodiment of the present application further provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the various steps in the above method embodiment can be implemented.

[0139] It should be understood that the above-mentioned specific embodiments of the present application are exemplary, and those skilled in the art can implement them individually or combine the methods between the embodiments to implement them.

[0140] The explanation of the relevant contents and beneficial effects of any of the above-mentioned devices can be referred to the corresponding method embodiments provided above, which will not be repeated here.

[0141] The above description is merely a specific embodiment of the present application, but the scope of protection of the present application is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in this application should be included in the scope of protection of this application. Therefore, the scope of protection of this application should be based on the scope of protection of the claims.

Claims

1. A robot positioning method, It is characterized in that The method comprises: Obtaining motion parameters corresponding to the current frame point cloud and the previous frame point cloud collected by the robot, wherein the motion parameters at least include linear velocity and angular velocity; Perform at least angle filtering, linear filtering and voxel filtering on the current frame point cloud to obtain the current frame optimized point cloud; wherein the angle filtering is used to remove noise points in the current frame point cloud, the linear filtering is used to extract structured scenes from the current frame point cloud, and the voxel filtering is used to cluster feature points in the current frame point cloud; According to the motion parameters corresponding to the current frame optimized point cloud and the previous frame point cloud, the corresponding position and posture of the robot when collecting the current frame point cloud is calculated to achieve positioning.

2. The method according to claim 1, It is characterized in that The angle filtering includes: Read the preset parameter configuration table to obtain relevant parameters, wherein the relevant parameters include a minimum filtering angle and a maximum filtering angle; The angle between the current feature point of the current frame point cloud and the feature point in the previous frame point cloud that matches the current feature point is calculated. If the angle is less than the minimum filtering angle or the angle is greater than the maximum filtering angle, the corresponding feature point is removed from the current frame point cloud.

3. The method according to claim 1 or 2, It is characterized in that The linear filtering comprises: Selecting some points in the current frame point cloud and the previous frame point cloud to construct a straight line and calculating the straight line equation of the current straight line based on the coordinates of the some points; Adding the remaining points in the point cloud that are not used to construct the straight line to the current straight line, and calculating the distance from the current added point to the current straight line based on the coordinates of the current added point and the straight line equation of the current straight line; If the distance from the current added point to the current straight line is greater than the preset distance threshold, the current added point is eliminated and the next point is calculated; conversely, if the distance from the current added point to the current straight line is within the preset distance threshold range, the linear equation of the current straight line is recalculated by linear fitting.

4. The method according to claim 3, It is characterized in that The linear filtering further comprises: If the distances between all currently added points and the current straight line are outside the preset distance threshold range, some points are reselected in the current frame point cloud and the previous frame point cloud to reconstruct the straight line.

5. The method according to claim 1 or 2, It is characterized in that The voxel filtering includes: A voxel grid is created for the point cloud of the current frame, and the feature points contained in each of the voxel grids are replaced with the centroid or center of the feature points contained in each of the voxel grids.

6. The method according to claim 1 or 2, It is characterized in that The step of calculating the corresponding position and posture of the robot when collecting the current frame point cloud according to the motion parameters corresponding to the current frame point cloud and the previous frame point cloud includes: Matching the current frame optimized point cloud with a preset map to obtain an initial pose corresponding to the current frame point cloud; the preset map is obtained by splicing historical frame point clouds obtained by scanning the scene; Based on the initial pose and motion parameters corresponding to the previous frame point cloud, obtaining the predicted pose corresponding to the current frame point cloud; The initial posture is corrected using the predicted posture of the current frame point cloud to obtain the posture of the robot when collecting the current frame point cloud.

7. The method according to claim 6, It is characterized in that The step of obtaining the predicted pose corresponding to the point cloud of the current frame based on the initial pose and motion parameters corresponding to the point cloud of the previous frame includes: The initial pose, motion parameters and preset motion model of the previous frame point cloud are used to predict the pose corresponding to the current frame point cloud, and the predicted pose corresponding to when the robot collects the current frame point cloud is obtained; the preset motion model is a two-wheel differential model, and the motion parameters include the linear speed of the left and right wheels of the robot when collecting the previous frame point cloud, including: Calculating the angular velocities of the centers of the left and right wheels when collecting the last frame of point cloud according to the linear velocities of the left and right wheels when collecting the last frame of point cloud; The time interval between the last frame of point cloud collected by the robot and the current frame of point cloud is obtained, and the linear velocity and angular velocity corresponding to the time interval when the robot collected the last frame of point cloud are used to calculate the predicted position and posture of the robot when collecting the current frame of point cloud.

8. The method according to claim 6 or 7, It is characterized in that The method of correcting the initial posture by using the predicted posture of the current frame point cloud to obtain the posture of the robot when collecting the current frame point cloud includes: Obtain the current system noise of the robot, use the predicted pose of the current frame point cloud and the system noise for nonlinear modeling, and obtain the estimated pose corresponding to the initial pose of the current frame point cloud; The difference between the initial pose and the estimated pose is calculated and combined with the Kalman gain to correct the predicted pose to obtain the pose of the robot when collecting the current frame point cloud.

9. A robot, It is characterized in that include: A point cloud acquisition device, used to collect point cloud data of the robot's surrounding environment; A motion acquisition device, used for synchronously acquiring motion parameters of the robot while the point cloud acquisition device acquires point cloud data, wherein the motion parameters include linear velocity and / or angular velocity; A main control chip is used to process the point cloud data collected by the point cloud acquisition device and the motion parameters collected by the motion acquisition device according to the robot positioning method described in any one of claims 1 to 8 to obtain the positioning information of the robot.

10. A computer-readable storage medium, It is characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed, the method according to any one of claims 1 to 8 is implemented.

Images