Robot positioning method and device, robot and readable storage medium

Abstract

The invention relates to the technical field of positioning, and discloses a robot positioning method and device, a robot and a readable storage medium, and the method comprises the steps: obtaining the first normal vector information of a current point cloud in response to the current environment point cloud collected by the robot in the advancing process; performing point cloud matching on the current point cloud and a constructed environment map to obtain a real-time pose of the robot in the environment map; wherein the environmental map comprises second normal vector information of each grid point, and the point cloud matching degree further comprises a normal vector matching item between the first normal vector information of the point cloud and the second normal vector information of the environmental map; and according to the obtained real-time pose, registering the current point cloud into the constructed environment map to update the environment map. According to the scheme, plane features with polyhedron can be prevented from being recognized as a single plane, so that the positioning precision of the robot and the map building precision are improved.

Description

Technical Field

[0001] The present application relates to the field of positioning technology, and in particular to a robot positioning method, device, robot, and readable storage medium. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) is commonly used in robotics. Its goal is to enable a robot to simultaneously build a map in an unknown environment and determine its own position within that map. Existing SLAM technology primarily matches points with a map. When applied to indoor scenes, the common presence of thin walls, doors, and other objects can easily lead to the misidentification of planar structures on both sides as a single plane. This reduces map accuracy and, in turn, affects the global consistency of the map. Summary of the Invention

[0003] In view of this, the present application provides a robot positioning method, device, robot and readable storage medium, which can improve the accuracy of robot positioning and map construction, etc.

[0004] In a first aspect, an embodiment of the present application provides a robot positioning method, comprising: In response to a current environment point cloud collected by the robot during movement, obtaining first normal vector information of the current point cloud; Performing point cloud matching on the current point cloud with the constructed environment map to obtain the real-time position and posture of the robot in the environment map; wherein the environment map includes second normal vector information of each grid point, and the point cloud matching degree between the point cloud and the environment map also includes a normal vector matching item between the first normal vector information of the point cloud and the second normal vector information of the environment map; According to the acquired real-time pose, the current point cloud is registered into the constructed environment map to update the environment map.

[0005] In some embodiments, obtaining first normal vector information of the point cloud includes: Searching for a neighborhood point set of each scan point in the current point cloud within a preset radius with the scan point as the origin, determining a corresponding normal vector calculation method based on the number of points in the neighborhood point set of each scan point, and thereby obtaining a normal vector for each scan point; Wherein, when the number of the points is two, the normal vector calculation method is: taking the vertical direction of the line connecting the two points in the neighborhood point set as the normal vector of the current scanning point; When the number of points is greater than two, the normal vector calculation method is: performing principal component analysis on all points in the neighborhood point set to calculate the normal vector of the current scanning point.

[0006] In some embodiments, performing principal component analysis on all points in the neighborhood point set to calculate the normal vector of the current scanning point includes: Obtaining the average value of all points in the neighborhood point set to calculate the difference between each point and the average value, and obtaining a difference point set with the average value as the origin; The covariance matrix of all points in the difference point set is obtained, and the eigenvector corresponding to the minimum eigenvalue of the covariance matrix is used as the normal vector of the current scanning point.

[0007] In some embodiments, the environment map is a probabilistic grid map of the environment; performing point cloud matching on the current point cloud with the constructed environment map to obtain the real-time position and posture of the robot in the environment map includes: Estimate the relative position and posture of the robot at the current moment based on the currently acquired sensor data of the robot and the position and posture information of the robot at the previous moment; Taking the relative posture as the initial posture, the current point cloud and the constructed probability grid map are subjected to a correlation scan matching operation with the normal vector matching item added to obtain the current optimal posture of the robot in the probability grid map and use it as the real-time posture.

[0008] In some embodiments, performing a correlation scan matching operation on the current point cloud and the constructed probability grid map with the normal vector matching item added includes: Searching for a plurality of candidate poses within a preset search window with the initial pose as the origin; For each candidate pose, projecting the current point cloud onto the constructed probabilistic grid map through the corresponding candidate pose to obtain a sum of occupancy probability values of the point cloud on the grid map, and obtaining a normal vector match between a first normal vector of the point cloud and a second normal vector of a grid point in the probabilistic grid map; Calculating a corresponding point cloud matching degree based on the sum of the occupancy probability values and the normal vector matching item; The candidate pose that maximizes the point cloud matching degree between the point cloud and the probability grid map is used as the current optimal pose.

[0009] In some embodiments, obtaining a normal vector match between a first normal vector of the point cloud and a second normal vector of a grid point in the probability grid map includes: Obtaining a dot product result of the first normal vector of each scanning point in the point cloud and the second normal vector of the corresponding grid point on the probability grid map; When the dot product result is greater than zero, setting the value of the normal vector matching item corresponding to the scanning point to a first value; When the dot product result is less than or equal to zero, a local consistency check is performed on the corresponding scanning point, and the obtained check result is used as the value of the normal vector matching item of the corresponding scanning point.

[0010] In some embodiments, the performing local consistency detection on the corresponding scanning points includes: Obtaining a ratio of successful matching of normal vectors of adjacent scanning points within a preset range of the scanning point as a detection result of local consistency detection; If the first normal vector of the scanning point is in the same direction as the second normal vector of the corresponding grid point on the probability grid map, the matching is confirmed to be successful; otherwise, the matching fails.

[0011] In some embodiments, after obtaining the optimal posture, the method further includes: Taking the optimal posture as the optimization basis, a nonlinear optimization function is used to smoothly transform each scanning point in the current point cloud into a map coordinate system with higher precision, so as to obtain the positioning posture of the robot in the environment map with sub-pixel precision.

[0012] In some embodiments, estimating the relative posture of the robot at the current moment based on the currently acquired sensor data and the posture information of the robot at the previous moment includes: Obtaining a translation amount of the robot from the previous moment to the current moment based on the acquired odometer data of the robot at the previous moment and the current moment; Obtaining a rotation amount of the robot from the previous moment to the current moment based on the acquired inertial measurement unit data of the robot at the previous moment and the current moment, and a posture difference between the inertial measurement unit data and the gravity direction; Based on the position of the robot at the previous moment and the translation amount, the relative position of the robot at the current moment is estimated; and based on the posture of the robot at the previous moment and the rotation amount, the relative posture of the robot at the current moment is estimated.

[0013] In some embodiments, the environment map is a probabilistic grid map of the environment; and registering the current point cloud with the constructed environment map based on the acquired real-time pose to update the environment map includes: updating the probability value of occupancy or vacancy of each grid point in the probability grid map according to the acquired real-time posture; Obtain a transformation matrix corresponding to the real-time posture, and use the transformation matrix to convert the first normal vector of the currently acquired point cloud from the robot coordinate system to the map coordinate system to obtain the direction vector of the current observation value; and update the second normal vector of each grid point in the probability grid map at the current moment according to the second normal vector information of each grid point in the probability grid map at the previous moment and the direction vector of the current observation value.

[0014] In a second aspect, an embodiment of the present application further provides a robot positioning device, comprising: A normal vector acquisition module, configured to obtain first normal vector information of a current point cloud in response to a current environment point cloud collected by the robot during movement; a matching and positioning module, configured to perform point cloud matching between the current point cloud and a constructed environment map to obtain the real-time position and posture of the robot in the environment map; wherein the environment map includes second normal vector information of each grid point, and the point cloud matching degree between the point cloud and the environment map also includes a normal vector matching item between the first normal vector information of the point cloud and the second normal vector information of the environment map; A map updating module is used to register the current point cloud into the constructed environment map according to the acquired real-time posture to update the environment map.

[0015] In a third aspect, an embodiment of the present application further provides a robot, comprising a laser radar, a processor and a memory, wherein the laser radar is used to collect environmental point clouds while the robot is moving, the memory stores a computer program, and the processor is used to execute the computer program to implement the above-mentioned robot positioning method.

[0016] In a fourth aspect, an embodiment of the present application further provides a readable storage medium storing a computer program, which, when executed, implements the robot positioning method.

[0017] The embodiments of the present application have the following advantages: The robot positioning method proposed in the embodiment of the present application calculates the normal vector of the laser point cloud of the environment collected in real time during the robot's movement to obtain the normal vector information of the point cloud; then, the latest point cloud obtained each time is matched with the currently constructed environment map based on the normal vector, that is, the normal vector matching items between the normal vector information of the point cloud and the normal vector information of the grid points in the environment map are also considered during the point cloud matching, so as to determine the real-time position and posture of the robot. Finally, according to the real-time position and posture obtained, the current point cloud is registered with the constructed environment map to update the environment map. The above operation can improve the positioning accuracy of the robot in the environment map; at the same time, since the environment map constructed by this application also contains the normal vector information of each grid point, the global consistency of the map can be improved during map construction, thereby achieving the purpose of improving map accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following is a brief introduction to the drawings required for use in the embodiments. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other relevant drawings can be obtained based on these drawings without creative work.

[0019] Figure 1 A schematic structural diagram of a robot according to an embodiment of the present application is shown; Figure 2 A first flow chart of the robot positioning method according to an embodiment of the present application is shown; Figure 3 A second flow chart of the robot positioning method according to an embodiment of the present application is shown; Figure 4 A third flow chart of the robot positioning method according to an embodiment of the present application is shown; Figure 5 The figure shows a point cloud with normal vector information obtained by the method of the embodiment of the present application; Figure 6 (a) shows the environment map constructed based on the traditional SLAM method; FIG6 ( b ) shows an environment map constructed based on the method of an embodiment of the present application; Figure 7 A schematic structural diagram of a robot positioning device according to an embodiment of the present application is shown.

[0020] Description of main component symbols: 10 - robot; 11 - processor; 12 - memory; 13 - perception unit; 100 - robot positioning device; 110 - normal vector acquisition module; 120 - matching positioning module; 130 - map update module. DETAILED DESCRIPTION

[0021] The technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments.

[0022] The components of the embodiments of the present application generally described and illustrated in the drawings herein may be arranged and designed in a variety of different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the claimed application, but rather merely represents selected embodiments of the present application. All other embodiments obtained by those skilled in the art based on the embodiments of the present application without creative effort are within the scope of protection of the present application.

[0023] Hereinafter, the terms "including", "having" and their cognates, which may be used in various embodiments of the present application, are intended only to indicate specific features, numbers, steps, operations, elements, components or combinations of the foregoing items, and should not be understood as first excluding the existence of one or more other features, numbers, steps, operations, elements, components or combinations of the foregoing items or the possibility of adding one or more features, numbers, steps, operations, elements, components or combinations of the foregoing items.

[0024] Furthermore, the terms “first,” “second,” “third,” etc., are merely used for distinguishing descriptions and are not to be understood as indicating or implying relative importance.

[0025] Unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by those skilled in the art to which the various embodiments of the present application belong. The terms (such as those defined in generally used dictionaries) will be interpreted as having the same meaning as in the context of the relevant technical field and will not be interpreted as having an idealized meaning or an overly formal meaning unless clearly defined in the various embodiments of the present application.

[0026] Figure 1 A schematic diagram of the structure of a robot 10 according to an embodiment of the present application is shown. For example, the robot 10 includes a processor 11, a memory 12, and a perception unit 13. The perception unit 13 is configured to perceive information about the environment in which the robot 10 resides, such as a point cloud of the environment during movement. The memory 12 stores a computer program, and the processor 11 executes the computer program to enable the robot 10 to perform more accurate robot positioning and environmental mapping according to the robot positioning method of the following embodiment.

[0027] The processor 11 may be an integrated circuit chip with signal processing capabilities. The processor 11 may be a general-purpose processor, including at least one of a central processing unit (CPU), a graphics processing unit (GPU), a network processor (NP), a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. The general-purpose processor may be a microprocessor or any conventional processor, and may implement or execute the various methods, steps, and logic block diagrams disclosed in the embodiments of this application.

[0028] The memory 12 may be, but is not limited to, a random access memory (RAM), 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), etc. The memory 12 is used to store a computer program, and the processor 11 may execute the computer program accordingly after receiving an execution instruction.

[0029] The sensing unit 13 is primarily used to perceive information about the environment in which the robot 10 is traveling. For example, for a sweeping robot, it may include a laser radar installed on the robot 10. The laser radar scans the surrounding environment along the travel path, generates a corresponding laser point cloud, and then provides it to the robot 10's control system for map construction and self-positioning. In some embodiments, the sensing unit 13 may also include a distance sensor, a visual sensor (such as a camera), etc., which is not limited here and can be configured according to actual needs.

[0030] It can be understood that the existence form of the above-mentioned robot 10 is not specifically limited. For example, it can include but is not limited to a sweeping robot, a swimming pool robot, a humanoid robot, a wheeled robot, etc. In other words, the method of the present application is universal and can be applied to various robots 10 and corresponding scenarios that support simultaneous positioning and map building technology, so as to improve the positioning capability and map building accuracy of the robot 10.

[0031] It is understood that when the robot 10 is in an unknown environment, it can use SLAM technology to simultaneously construct a map and determine its own positioning information within the map. In this application, based on SLAM technology, on the one hand, the structure of the constructed environmental map is improved, so that the constructed environmental map contains normal vector information. For example, taking the construction of a probabilistic grid map as an example, in addition to storing the occupancy probability value of each grid point, the normal vector of each grid point is also synchronously updated and stored. On the other hand, when matching the latest point cloud acquired each time with the constructed environmental map, a normal vector matching item is added between the point cloud and the probabilistic grid map to evaluate the consistency of the normal vector direction. It is understood that because the environmental map contains normal vector information, it can avoid misidentifying multi-faceted planar structures (including two or more sides) as a single plane, thereby improving the accuracy of the robot's map construction. At the same time, by adding normal vector information to assist in matching each time the latest point cloud is matched, the robot's positioning accuracy within the environmental map can be improved.

[0032] The robot positioning method is described below with reference to some specific embodiments.

[0033] Figure 2 A flow chart of a robot positioning method according to an embodiment of the present application is shown. Exemplarily, the robot positioning method includes the following steps: S11 , in response to a current environment point cloud collected by the robot during movement, obtaining first normal vector information of the current point cloud.

[0034] For example, when the robot 10 moves in an environment such as an indoor environment, it can use sensors such as a laser radar installed on the robot to scan the robot's current surroundings in real time and obtain corresponding point cloud data (also called environmental point cloud). Then, through analysis and processing, the robot 10 can know where the walls are, where the furniture is, etc., and also know where its current location is, etc.

[0035] Among them, a frame of point cloud obtained by each scan includes many scanning points. Correspondingly, when calculating the point cloud normal vector, each scanning point in the frame point cloud will be used as an object, and the normal vector characteristics of each scanning point will be calculated separately. In order to conveniently distinguish the normal vector information of the point cloud from the normal vector information of the grid points in the environmental map, the normal vector of the point cloud is recorded as the first normal vector, and the normal vector of the grid point is recorded as the second normal vector. It can be understood that this application uses the normal vector information of the point cloud to assist in identifying various objects in the room, especially planar structures with two or more sides, such as doors, thin walls, etc., to avoid misidentifying the above-mentioned planar structures as a single plane, thereby improving the positioning accuracy based on SLAM technology and ensuring the global consistency of the environmental map.

[0036] For example, in one embodiment, obtaining the first normal vector information of the current point cloud includes: Within a preset radius with the scanning point as the origin, the neighborhood point set of each scanning point in the current point cloud is searched to determine the corresponding normal vector calculation method according to the number of points in the neighborhood point set of each scanning point, and then obtain the first normal vector of each scanning point.

[0037] The neighborhood point set refers to the set of all points within the neighborhood of the current scan point. In other words, for each scan point, an appropriate search radius is selected with that scan point as the origin to form a neighborhood (region). A search algorithm is then used within this region to search for other points within close proximity to the current scan point in real time to generate the neighborhood point set for the current scan point. For example, the KD-Tree algorithm, R-Tree algorithm, K-Means Tree algorithm, or other improved or derived nearest neighbor search algorithms can be used, but these are not limited here. The selection of the preset radius can be based on actual conditions, such as the angular resolution of the lidar sensor, and is not limited here.

[0038] Since the number of scanning points contained in the neighborhood point set of each scanning point may be different, this embodiment will use different normal vector processing methods for different point numbers. For example: In the first case, when there are two points in the neighborhood point set, it indicates that there is an adjacent point near the current scanning point. At this time, the normal vector calculation method can be: the vertical direction of the line connecting the two points in the neighborhood point set is used as the normal vector of the current scanning point, that is, first calculate the line connecting the two points and then take the direction vector perpendicular to the line as its normal vector feature.

[0039] In the second case, when the number of points in the neighborhood point set is greater than two, such as three, four, or more, the normal vector calculation method may be to perform principal component analysis on all points in the neighborhood point set to calculate the normal vector of the current scan point. It will be appreciated that the principal component analysis algorithm is employed here to achieve dimensionality reduction while preserving the distribution characteristics of the original multiple scan points as much as possible, thereby calculating the normal vector characteristics of the current scan point.

[0040] Exemplarily, the average value of all points in the neighborhood point set of the current scanning point is obtained to calculate the difference between each point and the average value, and a difference point set with the average value as the origin is obtained; then, the covariance matrix of all points in the difference point set is obtained, and then, the eigenvector corresponding to the minimum eigenvalue of the covariance matrix is used as the normal vector of the current scanning point.

[0041] For example, taking the collected two-dimensional point cloud data as an example, assuming that the neighborhood point set is recorded as P and the generated difference point set is recorded as P', for the above principal component analysis algorithm process, there is a calculation formula for first calculating the average value of all points, as follows: ; Where, ( , ) is the coordinate of the i-th scanning point in the neighborhood point set P in the xy plane, is the number of all points in the neighborhood point set P; Indicates averaging all points in the x direction. Indicates averaging all points in the y direction; ( , ) is the coordinate of the point in the difference point set P' corresponding to the i-th scanning point in the xy plane.

[0042] Then, calculate the covariance matrix and solve the corresponding eigenvector: ; ; Where, Represents the covariance matrix of all points in the difference point set; is a smaller eigenvalue; It is understood that if the collected data is a 3D point cloud, the same principle of the above normal vector calculation method is applicable. Since the 3D point cloud also includes the value of the z direction, the calculation amount will be greater.

[0043] In addition, regarding the number of points in the above-mentioned neighborhood point set, there may be a third situation, that is, when the number of points is one, it indicates that the scanning point is far away from other scanning points, which may be generated due to laser noise. As an optional solution, the current scanning point can be discarded at this time.

[0044] It can be understood that through the above-mentioned normal vector calculation, the first normal vector information of each scanning point in each frame point cloud can be obtained. In addition, since some noise points can be excluded while calculating the first normal vector, this also provides a more accurate data basis for subsequent map construction and self-positioning.

[0045] S12, performing point cloud matching on the current point cloud and the constructed environment map to obtain the real-time position and posture of the robot in the environment map; wherein, the environment map includes the second normal vector information of each grid point, and the point cloud matching degree between the current point cloud and the environment map also includes the normal vector matching item between the first normal vector information of the current point cloud and the second normal vector information of the environment map.

[0046] In this embodiment, improvements are made based on SLAM technology to achieve more accurate robot self-positioning and map construction. Among them, the core of SLAM technology mainly includes perception, positioning, construction, and so on. Figure 3 The robot uses sensors (lidar or visual sensors) to obtain information about the surrounding environment. Positioning refers to obtaining its own position and posture in real time through sensors. Mapping refers to describing the current environment based on its own position and the information obtained by sensors.

[0047] Point cloud matching refers to finding the corresponding position of a frame of point cloud in the currently acquired local environment on the constructed environment map, that is, matching and splicing it into the constructed environment map. At the same time, the positioning information of the robot 10 itself in the environment map can be determined. The point cloud matching degree refers to the degree of alignment between the current point cloud and the currently constructed environment map. The normal vector matching term is used to indicate the degree of match between the first normal vector of the point cloud and the second normal vector of the grid point in the probabilistic grid map. The difference in this embodiment is that when performing point cloud matching, not only the position information of each scanning point is considered, but also the normal vector matching term between the point cloud and the environment map is introduced, which can more accurately identify the multifaceted nature of the planar structure.

[0048] In one embodiment, Figure 3 As shown, the real-time position of the robot in the environment map is obtained, including: S21, estimating the relative posture of the robot at the current moment based on the currently acquired sensor data of the robot and the posture information of the robot at the previous moment.

[0049] Among them, the estimation of relative pose is mainly used as a preparation for subsequent correlation scan matching. Since the robot needs to align the currently acquired point cloud data with the existing map, the relative pose is used as prior information to provide a starting point close to the true pose, so that subsequent matching only needs to search within a smaller range instead of exhaustively searching the entire map, so as to speed up the search for the best matching pose, thereby significantly improving efficiency.

[0050] The aforementioned sensor data primarily includes odometry data and inertial measurement unit (IMU) data. By combining the previous moment's position and the sensor data acquired at the current moment, a reasonable relative position can be estimated. For example, the robot's translation from the previous moment to the current moment can be obtained based on the robot's odometry data at the previous and current moments; and the robot's rotation from the previous moment to the current moment can be obtained based on the robot's IMU data at the previous and current moments, and the attitude difference between the IMU data and the direction of gravity. Furthermore, based on the robot's position and translation at the previous moment, the robot's relative position at the current moment is estimated; and based on the robot's attitude and rotation at the previous moment, the robot's relative attitude at the current moment is estimated.

[0051] It can be understood that the IMU is usually calibrated when the robot state is initialized. However, with use or external factors, the IMU may have a deviation between the z direction measured by it and the gravity direction. To ensure the accuracy of the data, this embodiment also calculates the posture difference between the IMU data and the gravity direction in real time for calibrating the IMU data.

[0052] For example, if the robot's position (position T and posture R) at the previous moment t-1 is recorded as ξ t-1 =(R t-1 , T t-1 ), the estimated pose at the current time t is recorded as ξ t =( R t , T t ), then: ; in, , .

[0053] Where, represents the translation amount, represents the amount of rotation; i∈[0,+∞), Indicates the t-1 arrive t The time between i-1 Hedi i The linear speed calculated from the odometer data, For the i-1 and i The time difference of odometer data; Indicates the t-1 arrive t The time between i-1 Hedi iThe angular velocity calculated from the inertial measurement unit data, For the i-1 and i The time difference of the inertial measurement unit data, For the i The attitude difference between the inertial measurement unit data and the direction of gravity.

[0054] S22, using the relative posture as the initial posture, performs a correlation scan matching operation with the normal vector matching item on the current point cloud and the constructed probability grid map of the environment to obtain the current optimal posture of the robot in the probability grid map and use it as the real-time posture.

[0055] Among them, correlation scan matching refers to aligning the latest point cloud data currently acquired with the constructed map to determine the current posture (position and direction) of the robot. For example, taking the construction of a probabilistic grid map as an example, the point cloud matching degree between the two can be quantified by calculating the sum of the occupancy probability values of the point cloud on the map. Among them, the probabilistic grid map is a two-dimensional or three-dimensional grid map used to represent the environment. Each grid cell represents the probability of the position being occupied, also known as an occupancy grid map. It is worth noting that when this embodiment uses the correlation scan matching algorithm for matching, it will also combine normal vector information for auxiliary matching, that is, when calculating the point cloud matching degree, a normal vector matching item between the point cloud and the probabilistic grid map is also added. The normal vector matching item can be used to evaluate the consistency of the normal vector direction, thereby improving the positioning accuracy of the robot.

[0056] In one embodiment, Figure 4 As shown, the current point cloud is subjected to a correlation scan matching operation with the probability grid map with a normal vector matching item added, including: S31, searching and obtaining a plurality of candidate poses within a preset search window with the initial pose as the origin.

[0057] The preset search window encompasses three dimensions: two-dimensional linear space and one-dimensional angular space. By using the preset spatial search range and traversal step size, a finite set of candidate poses can be obtained. As can be appreciated, by designing a search window and traversing the limited candidate poses within it, reasonable matches can be found in a shorter time, thereby improving search efficiency. Furthermore, even in the presence of noise or partial occlusion, good matches can be found through optimization.

[0058] S32, for each candidate pose, project the current point cloud onto a known probability grid map through the corresponding candidate pose to obtain the sum of the occupancy probability values of the point cloud on the probability grid map, and obtain the matching item between the first normal vector of the point cloud and the second normal vector of the grid point in the probability grid map.

[0059] Exemplarily, for each candidate pose, two items need to be calculated: the sum of the occupancy probability values. For example, this can be accomplished by transforming the currently acquired point cloud from the robot coordinate system to the map coordinate system using a transformation matrix, thereby calculating the sum of the corresponding probabilities for each scan point to the nearest grid point in the probabilistic grid map. The second item is the normal vector matching term. The combined results of these two items are then used to quantify the point cloud matching degree of the candidate pose. It should be understood that the normal vector matching term here is primarily used to adjust the sum of the occupancy probability values in the previous term. It should be understood that the constructed probabilistic grid map is constructed using historical point clouds and corresponding point cloud normal vectors. That is, the probabilistic grid map includes not only the occupancy probability values of each grid point, but also the normal vector information (second normal vector information) for each grid point. The second normal vector is primarily calculated using the first normal vector of the point cloud and the transformation matrix corresponding to the matched optimal pose. It should be understood that if the current point cloud is projected onto the grid map, the second normal vector information for the corresponding grid point can be obtained, allowing normal vector direction consistency testing to be performed.

[0060] For the normal vector matching item, exemplarily, the result of the dot product of the normal vector of the current point cloud and the probability grid map can be obtained; then, the value of the normal vector matching item is determined based on the scan dot product result. Specifically, the dot product result of the first normal vector of each scan point in the point cloud and the second normal vector of the corresponding grid point on the probability grid map is calculated; for any scan point, for example, when its dot product result is greater than zero, it indicates that the direction of the first normal vector of the scan point is consistent with the direction of the second normal vector of the corresponding grid point, and then the value of the normal vector matching item of the scan point can be set to the first value (such as 1). Conversely, when the dot product result is less than or equal to zero, it indicates that the direction of the first normal vector of the scan point is inconsistent with the direction of the second normal vector of the corresponding grid point, and then the scan point can be further subjected to a local consistency test, and the obtained test result is used as the value of the normal vector matching item of the scan point.

[0061] If the first normal vector of the scan point aligns with the second normal vector of the corresponding grid point on the probability grid map, the match is considered successful; otherwise, the match fails. It can be understood that the purpose of local consistency checking is to ensure global consistency by further counting the matching results of neighboring points for scan points that failed to match. Exemplarily, the local consistency check result can be obtained by calculating the proportion of successfully matched normal vectors of neighboring point clouds within a preset range of the scan point, which is then assigned to the normal vector matching term.

[0062] In one embodiment, if described by an expression, there is: Where, V c Indicates the number of successful matches. Tc Indicates the number and ratio of successful and failed matches r The value range of is [0, 1).

[0063] S33: Calculate the corresponding point cloud matching degree based on the sum of the occupancy probability values and the normal vector matching item.

[0064] It can be understood that by calculating the corresponding point cloud matching degree under each candidate posture, the candidate posture with the largest point cloud matching degree is taken as the optimal posture of the current robot in the environment map, that is, the current positioning posture of the robot that needs to be solved.

[0065] S34, taking the candidate pose that maximizes the point cloud matching degree between the point cloud and the probability grid map as the current optimal pose.

[0066] For example, in one embodiment, if described by an expression, there is: ; Where, is the optimal posture, is the candidate pose, W Represents the search window, Represents the transformation matrix corresponding to the candidate pose, which is used to transform the point cloud from the robot coordinate system to the map coordinate system, s p represents the p-th scan point in the point cloud, Indicates the number of scan points of the current point cloud, function M nearest Indicates rounding its parameters to the nearest grid point and obtaining its probability value on the probability grid map; n p Represents the first normal vector of the p-th scan point in the point cloud, function N Indicates obtaining the second normal vector of the corresponding grid point on the probability grid map.

[0067] As a preferred solution, after obtaining the above-mentioned pixel-accurate real-time optimal posture of the robot in the environment map, in order to further optimize the accuracy of the map, the method further includes: Using the optimal posture as the optimization basis, a nonlinear optimization function is used to smoothly transform each scanning point in the current point cloud into a map coordinate system with higher accuracy (sub-pixel level) to obtain the positioning posture of the robot in the environment map with sub-pixel accuracy.

[0068] It is understood that in order to smoothly convert the probability values of the laser point cloud in the discrete probability grid map to the more accurate map coordinate system, this embodiment uses an interpolation algorithm to achieve this, thereby supporting sub-pixel positioning accuracy. For example, the originally acquired positioning pose is (1, 2), while the converted positioning pose can be accurate to one or more decimal places, such as (1.2, 2.1).

[0069] Exemplarily, the interpolation algorithm may be, but is not limited to, bicubic interpolation, bilinear interpolation, biquadratic interpolation, spline interpolation, etc. Bicubic interpolation is an interpolation method for two-dimensional data that uses a cubic polynomial to interpolate at each grid point and estimates the value of the target point by fitting a continuous and differentiable surface.

[0070] For example, in one embodiment, the nonlinear optimization function may adopt the following formula: ; Where, is the positioning pose with sub-pixel accuracy, Represents the transformation matrix, which is used to transform the scan points Convert from the robot coordinate system to the map coordinate system, Represents the bicubic interpolation function.

[0071] S13, according to the acquired real-time pose, registering the current point cloud into the constructed environment map to update the environment map.

[0072] Exemplarily, after the current point cloud is matched, the environment map can be updated based on the acquired real-time pose and the current point cloud. The update operation on the probability grid map can be understood as a map fusion operation, which splices the latest point cloud data from the lidar into the constructed environment map to gradually build a more complete global environment map. It is understood that after the point cloud scan matching is completed, the map update operation and the determination of the positioning pose can be performed. These two steps can be performed simultaneously or sequentially in a predetermined order, which is not limited here.

[0073] It can be understood that registering the current point cloud with the constructed environment map is essentially used to update the map. In this embodiment, the update includes two parts: updating the probability value of the grid point and updating the normal vector of the grid point. Exemplarily, the probability value of each grid point in the probability grid map can be updated based on the acquired real-time pose; and the transformation matrix corresponding to the real-time pose can be obtained to use the transformation matrix to transform the first normal vector of the current point cloud from the robot coordinate system to the map coordinate system to obtain the direction vector of the current observation value; and the second normal vector of each grid point in the probability grid map at the current moment is updated based on the second normal vector of each grid point in the probability grid map at the previous moment and the direction vector of the current observation value.

[0074] In one embodiment, the probability value of each grid point can be updated by calculating the probability calculation ratio of each grid point being occupied or idle under the new observation value, and then multiplying it by the previous probability calculation ratio. For example, this can be achieved as follows: ; ; Where, Represents the occupancy probability of a known grid point Divide by the idle probability proportion; and They represent the occupancy probability and idle probability of the grid point when there is a new observation value z; It can be understood that the new observation value z can be obtained by projecting the current point cloud into the map coordinate system through the optimal pose obtained.

[0075] In one embodiment, the normal vector of a grid point can be updated by adding the normal vector of the grid point in the probability grid map at the previous moment to the direction vector of the current observation value z, and then performing normalization to obtain an updated second normal vector. This method can gradually integrate new observation information, making the normal vector information of each grid point in the environment map more accurate. For example, this can be achieved as follows: ; Where, n m and n m-1 Represent the normal vector of the grid point at the current moment and the previous moment respectively, n z is the direction vector of the current observation value z, which is obtained by transforming the normal vector of the point cloud using the latest transformation matrix; function Indicates normalization processing.

[0076] In order to better reflect the effect of the technical solution of this application, the traditional SLAM method and the method of the embodiment of this application are respectively used for the same indoor environment to perform robot positioning and environmental map construction. For example, for a certain indoor area, Figure 5 The figure shows the laser point cloud data with normal vector information using the method of the embodiment of the present application, where the green arrow is the normal vector and the red point is the laser point cloud. In addition, Figure 6 (a) shows a grid map of the current environment constructed based on the traditional SLAM method, and Figure 6 (b) shows a grid map constructed using the method of the embodiment of the present application (with the assistance of normal vector information). It can be seen that the two planar structures in the lower left and upper middle are both identified as a single plane in Figure 6 (a), while in Figure 6 (b) they are identified as a planar structure with two planes. Therefore, it can be seen that for planar features with two sides such as thin walls and doors, the technical solution of the present application can achieve more accurate recognition.

[0077] The embodiment of the present application adds an additional information dimension of normal vector on the basis of SLAM positioning technology, which can help the robot better understand the planar features in the environment and avoid the misidentification of planar features. This not only improves positioning accuracy, but also ensures the quality of the constructed map, and is suitable for applications in scenarios such as indoor robot navigation.

[0078] Figure 7 A schematic structural diagram of a robot positioning device 100 according to an embodiment of the present application is shown. Exemplarily, the robot positioning device 100 includes: The normal vector acquisition module 110 is configured to obtain first normal vector information of the current point cloud in response to the point cloud of the current environment collected by the robot during movement; A matching and positioning module 120 is configured to perform point cloud matching between the current point cloud and the constructed environment map to obtain the real-time position and posture of the robot in the environment map; wherein the environment map includes the second normal vector information of each grid point, and the point cloud matching degree between the point cloud and the environment map also includes a normal vector matching item between the first normal vector information of the point cloud and the second normal vector information of the environment map; The map updating module 130 is used to register the current point cloud into the constructed environment map according to the acquired real-time pose to update the environment map.

[0079] As an optional solution, the normal vector acquisition module 110 is specifically used to search for the neighborhood point set of each scanning point in the point cloud within a preset radius range with the scanning point as the origin, so as to determine the corresponding normal vector calculation method according to the number of points in the neighborhood point set of each scanning point, and then obtain the normal vector of each scanning point.

[0080] As an optional solution, when obtaining the first normal vector of a scanning point, the normal vector acquisition module 110 is specifically used to, when the number of points is two, use the vertical direction of the line connecting the two points in the neighborhood point set as the first normal vector of the current scanning point; and, when the number of points is greater than two, perform principal component analysis on all points in the neighborhood point set to calculate the first normal vector of the current scanning point.

[0081] As an optional solution, the normal vector acquisition module 110 performs principal component analysis on all points in the neighborhood point set to calculate the first normal vector of the current scanning point, including: Get the average value of all points in the neighborhood point set to calculate the difference between each point and the average value, and obtain a difference point set with the average value as the origin; get the covariance matrix of all points in the difference point set, and use the eigenvector corresponding to the minimum eigenvalue of the covariance matrix as the first normal vector of the current scanning point.

[0082] As an optional solution, the environment map is a probabilistic grid map of the environment; the matching and positioning module 120 is specifically used to estimate the relative posture of the robot at the current moment based on the currently acquired sensor data of the robot and the posture information of the robot at the previous moment; and, using the relative posture as the initial posture, perform a correlation scanning and matching operation with the normal vector matching item on the current point cloud and the constructed probabilistic grid map of the environment to obtain the optimal posture of the robot in the probabilistic grid map and use it as the real-time posture.

[0083] As an optional solution, the matching and positioning module 120 is specifically used to search for several candidate poses within a preset search window with the initial pose as the origin; for each candidate pose, the current point cloud is projected onto a known probability grid map through the corresponding candidate pose to obtain the sum of the occupancy probability values of the point cloud on the grid map, and to obtain the normal vector matching item between the first normal vector of the point cloud and the second normal vector of the grid point in the probability grid map; finally, the corresponding point cloud matching degree is calculated based on the sum of the occupancy probability values and the normal vector matching item, and the candidate pose that maximizes the point cloud matching degree between the point cloud and the probability grid map is used as the current optimal pose.

[0084] As an optional solution, the matching and positioning module 120 is configured to obtain a normal vector matching item between the first normal vector of the point cloud and the second normal vector of the grid point in the probability grid map, including: Obtain the dot product result of the first normal vector of each scanning point in the point cloud and the second normal vector of the corresponding grid point on the probability grid map; when the dot product result is greater than zero, set the value of the normal vector matching item of the corresponding scanning point to the first value; when the dot product result is less than or equal to zero, perform local consistency detection on the corresponding scanning point, and use the obtained detection result as the value of the normal vector matching item of the corresponding scanning point.

[0085] As an optional solution, the matching and positioning module 120 performs local consistency detection on corresponding scanning points, including: The ratio of successful normal vector matching of adjacent point clouds within the preset range of the point cloud is obtained as the result of local consistency detection; among them, if the first normal vector of a scan point is consistent with the direction of the second normal vector of the corresponding grid point on the probability grid map, the match is confirmed to be successful, otherwise the match fails.

[0086] As an optional solution, after obtaining the optimal posture, the matching and positioning module 120 further includes: Taking the optimal pose as the optimization basis, the scanning points in the point cloud are smoothly converted into a more accurate map coordinate system using a nonlinear optimization function to obtain the positioning pose of the robot in the environment map with sub-pixel accuracy.

[0087] As an optional solution, the matching and positioning module 120 estimates the relative position of the robot at the current moment based on the currently acquired sensor data and the position information of the robot at the previous moment, including: Based on the odometer data of the robot at the previous moment and the current moment, the translation amount of the robot from the previous moment to the current moment is obtained; based on the inertial measurement unit data of the robot at the previous moment and the current moment, and the posture difference between the inertial measurement unit data and the gravity direction, the rotation amount of the robot from the previous moment to the current moment is obtained; based on the position and translation amount of the robot at the previous moment, the relative position of the robot at the current moment is estimated; and based on the posture and rotation amount of the robot at the previous moment, the relative posture of the robot at the current moment is estimated.

[0088] As an optional solution, the map update module 130 is specifically used to: update the probability value of occupation or vacancy of each grid point in the probability grid map according to the acquired real-time posture; obtain the transformation matrix corresponding to the real-time posture, and use the transformation matrix to convert the first normal vector of the current point cloud from the robot coordinate system to the map coordinate system to obtain the direction vector of the current observation value; and update the second normal vector of each grid point in the probability grid map at the current moment according to the second normal vector of each grid point in the probability grid map at the previous moment and the direction vector of the current observation value.

[0089] It is understood that the various modules in this embodiment correspond to corresponding steps in the robot positioning method of the above-mentioned embodiment. Therefore, the specific implementation details of each step can be found in the corresponding description of the above-mentioned embodiment and will not be repeated here. In addition, the optional options in the above-mentioned embodiment are also applicable to this embodiment and will not be repeated here.

[0090] This application also provides a computer-readable storage medium for storing a computer program used in the robot. For example, the computer-readable storage medium may include, but is not limited to, a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk, among other media capable of storing program code.

[0091] In the several embodiments provided in this application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and structure diagrams in the accompanying drawings show the possible architectures, functions and operations of the devices, methods and computer program products according to the multiple embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of the code, and the module, program segment or a part of the code contains one or more executable instructions for implementing the specified logical functions. It should also be noted that in an alternative implementation, the functions marked in the box can also occur in an order different from that marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the structure diagram and / or flowchart, and the combination of boxes in the structure diagram and / or flowchart, can be implemented using a dedicated hardware-based system that performs the specified function or action, or can be implemented using a combination of dedicated hardware and computer instructions.

[0092] In addition, the functional modules or units in the various embodiments of the present application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part.

[0093] If a function is implemented in the form of a software function module and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, or the part that contributes to the existing technology, or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes a number of instructions for enabling a computer device (which can be a smart phone, personal computer, server, or network device, etc.) to execute all or part of the steps of the various embodiments of the method of this application.

[0094] The above is only a specific implementation method of the present application, but the protection scope of the present application is not limited thereto. Any technician familiar with this technical field can easily think of changes or replacements within the technical scope disclosed in this application, which should be covered by the protection scope of the present application.

Claims

1. A robot positioning method, characterized in that: include: In response to a current environment point cloud collected by the robot during movement, obtaining first normal vector information of the current point cloud; Performing point cloud matching on the current point cloud with the constructed environment map to obtain the real-time position and posture of the robot in the environment map; wherein the environment map includes second normal vector information of each grid point, and the point cloud matching degree between the point cloud and the environment map also includes a normal vector matching item between the first normal vector information of the point cloud and the second normal vector information of the environment map; According to the acquired real-time pose, the current point cloud is registered into the constructed environment map to update the environment map.

2. The robot positioning method according to claim 1, characterized in that: The obtaining of the first normal vector information of the current point cloud includes: Searching for a neighborhood point set of each scan point in the current point cloud within a preset radius with the scan point as the origin, determining a corresponding normal vector calculation method based on the number of points in the neighborhood point set of each scan point, and thereby obtaining a normal vector for each scan point; Wherein, when the number of the points is two, the normal vector calculation method is: taking the vertical direction of the line connecting the two points in the neighborhood point set as the normal vector of the current scanning point; When the number of points is greater than two, the normal vector calculation method is: performing principal component analysis on all points in the neighborhood point set to calculate the normal vector of the current scanning point.

3. The robot positioning method according to claim 2, characterized in that: The performing principal component analysis on all points in the neighborhood point set to calculate the normal vector of the current scanning point includes: Obtaining the average value of all points in the neighborhood point set to calculate the difference between each point and the average value, and obtaining a difference point set with the average value as the origin; The covariance matrix of all points in the difference point set is obtained, and the eigenvector corresponding to the minimum eigenvalue of the covariance matrix is used as the normal vector of the current scanning point.

4. The robot positioning method according to claim 1, characterized in that: The environment map is a probabilistic grid map of the environment; performing point cloud matching on the current point cloud and the constructed environment map to obtain the real-time position and posture of the robot in the environment map includes: Estimate the relative position and posture of the robot at the current moment based on the currently acquired sensor data of the robot and the position and posture information of the robot at the previous moment; Taking the relative posture as the initial posture, the current point cloud and the constructed probability grid map are subjected to a correlation scan matching operation with the normal vector matching item added to obtain the current optimal posture of the robot in the probability grid map and use it as the real-time posture.

5. The robot positioning method according to claim 4, characterized in that: The performing a correlation scan matching operation on the current point cloud and the constructed probability grid map with the normal vector matching item added thereto includes: Searching for a plurality of candidate poses within a preset search window with the initial pose as the origin; For each candidate pose, projecting the current point cloud onto the constructed probabilistic grid map through the corresponding candidate pose to obtain a sum of occupancy probability values of the point cloud on the grid map, and obtaining a normal vector match between a first normal vector of the point cloud and a second normal vector of a grid point in the probabilistic grid map; Calculating a corresponding point cloud matching degree based on the sum of the occupancy probability values and the normal vector matching item; The candidate pose that maximizes the matching degree between the point cloud and the probability grid map is used as the current optimal pose.

6. The robot positioning method according to claim 5, characterized in that: The obtaining of a normal vector matching item between a first normal vector of the point cloud and a second normal vector of a grid point in the probability grid map includes: Obtaining a dot product result of the first normal vector of each scanning point in the point cloud and the second normal vector of the corresponding grid point on the probability grid map; When the dot product result is greater than zero, setting the value of the normal vector matching item corresponding to the scanning point to a first value; When the dot product result is less than or equal to zero, a local consistency check is performed on the corresponding scanning point, and the obtained check result is used as the value of the normal vector matching item of the corresponding scanning point.

7. The robot positioning method according to claim 6, characterized in that: The performing local consistency detection on the corresponding scanning points includes: Obtaining a ratio of successful normal vector matching of adjacent point clouds within a preset range of the scanning point as a detection result of local consistency detection; If the first normal vector of the scanning point is in the same direction as the second normal vector of the corresponding grid point on the probability grid map, the matching is confirmed to be successful; otherwise, the matching fails.

8. The robot positioning method according to claim 4, characterized in that: After obtaining the optimal posture, the method further includes: Taking the optimal posture as the optimization basis, a nonlinear optimization function is used to smoothly transform each scanning point in the current point cloud into a map coordinate system with higher precision, so as to obtain the positioning posture of the robot in the environment map with sub-pixel precision.

9. The robot positioning method according to claim 4, characterized in that: The estimating the relative posture of the robot at the current moment based on the currently acquired sensor data and the posture information of the robot at the previous moment includes: Obtaining a translation amount of the robot from the previous moment to the current moment based on the acquired odometer data of the robot at the previous moment and the current moment; Obtaining a rotation amount of the robot from the previous moment to the current moment based on the acquired inertial measurement unit data of the robot at the previous moment and the current moment, and a posture difference between the inertial measurement unit data and the gravity direction; Based on the position of the robot at the previous moment and the translation amount, the relative position of the robot at the current moment is estimated; and based on the posture of the robot at the previous moment and the rotation amount, the relative posture of the robot at the current moment is estimated.

10. The robot positioning method according to claim 1, characterized in that: The environment map is a probabilistic grid map of the environment; and the step of registering the current point cloud with the constructed environment map based on the acquired real-time pose to update the environment map includes: updating the probability value of occupancy or vacancy of each grid point in the probability grid map according to the acquired real-time posture; Obtain a transformation matrix corresponding to the real-time posture, and use the transformation matrix to convert the first normal vector of the current point cloud from the robot coordinate system to the map coordinate system to obtain the direction vector of the current observation value; and update the second normal vector of each grid point in the probability grid map at the current moment according to the second normal vector of each grid point in the probability grid map at the previous moment and the direction vector of the current observation value.

11. A robot positioning device, characterized in that: include: A normal vector acquisition module, configured to obtain first normal vector information of a current point cloud in response to a current environment point cloud collected by the robot during movement; a matching and positioning module, configured to perform point cloud matching between the current point cloud and a constructed environment map to obtain the real-time position and posture of the robot in the environment map; wherein the environment map includes second normal vector information of each grid point, and the point cloud matching degree between the point cloud and the environment map also includes a normal vector matching item between the first normal vector information of the point cloud and the second normal vector information of the environment map; A map updating module is used to register the current point cloud into the constructed environment map according to the acquired real-time posture to update the environment map.

12. A robot, characterized in that: The robot includes a laser radar, a processor and a memory, the laser radar is used to collect environmental point clouds during the movement of the robot, the memory stores a computer program, and the processor is used to execute the computer program to implement the robot positioning method according to any one of claims 1 to 10.

13. A computer-readable storage medium, characterized in that The device stores a computer program, which, when executed, implements the robot positioning method according to any one of claims 1 to 10.

Images