Positioning method for robot and robot using the same

Abstract

The present invention provides a positioning method for a robot, the robot includes a laser radar and an odometer, and the positioning method includes: obtaining the posture of the robot through the odometer; predicting the posture at the current moment according to the posture at the previous moment; obtaining the measured point cloud at the current moment through the laser radar; matching the measured point cloud with a map, and obtaining displacement confidence and rotation confidence according to the matching result; obtaining a corrected posture according to the matched point cloud, the displacement confidence and the rotation confidence; determining the posture at the current moment according to the predicted posture and the corrected posture. The present invention determines the current posture of the robot according to the predicted posture of the odometer and the corrected posture of the laser radar, and can also assign weights to the predicted posture and the corrected posture to reduce the errors generated in the prediction process and the correction process, which can avoid or reduce the occurrence of mismatching and increase the reliability and stability of the positioning result.

Description

Technical Field

[0001] The present disclosure relates to the technical field of robots, and in particular to a positioning method for a robot and a robot using the same. Background Art

[0002] With the rapid development of mobile robots, especially in the application fields of autonomous navigation vehicles, unmanned vehicles and service robots, various automated tasks, such as object grasping and space exploration, have put forward higher requirements for the positioning of mobile robots, and odometer technology plays an extremely important role in this. Among them, laser radar and wheel odometer are the most commonly used modules to improve the robot's perception of the environment and its own positioning.

[0003] The odometer uses the speed and angular velocity obtained from the observed left and right wheel speeds, substitutes them into the mobile robot kinematic model, and deduces the robot's current position and heading angle information. Due to measurement errors, actuator errors, model errors, and possible side slips, the odometer information also produces large errors. Since the odometer is continuously deduced using a geometric model, the errors will continue to accumulate in the results and eventually become unusable. However, the odometer will not drift when static and has the advantage of low cost.

[0004] In the robot positioning solution based on lidar and odometer, the robot's current posture is equal to the sum of the predicted posture of the odometer and the corrected posture of the lidar. However, there are errors in the process of predicting the posture of the odometer and correcting the posture of the lidar. When determining the final result, there is a problem of error accumulation. In addition, mismatching when matching laser information with the map will also lead to errors in the corrected posture, and the final positioning result is also wrong.

[0005] The contents of the background technology section merely disclose the technologies known to the inventors and do not necessarily represent the prior art in the field. Summary of the invention

[0006] In view of one or more existing defects, the present invention provides a positioning method for a robot, wherein the robot includes a laser radar and an odometer, and the positioning method includes:

[0007] Acquiring the position and posture of the robot through the odometer;

[0008] Predict the current position based on the previous position;

[0009] Acquire the point cloud measured at the current moment through the laser radar;

[0010] Matching the measured point cloud with the map, and obtaining displacement confidence and rotation confidence according to the matching result;

[0011] Acquire a corrected posture according to the matched point cloud, the displacement confidence and the rotation confidence;

[0012] The current posture is determined based on the predicted posture and the corrected posture.

[0013] According to one aspect of the present invention, the step of matching the measured point cloud with the map and obtaining the displacement confidence and rotation confidence based on the matching results specifically includes: matching the measured point cloud with the point cloud map of the current operating environment through the ICP algorithm, and obtaining the displacement confidence and rotation confidence based on the matching results.

[0014] According to one aspect of the present invention, the step of matching the measured point cloud with the map and obtaining the displacement confidence and rotation confidence based on the matching results specifically includes: determining the first direction and the second direction based on the normal direction of each point in the matched point cloud, and determining the displacement confidence based on the total number of points within a preset range of the first direction and the total number of points within a preset range of the second direction.

[0015] According to one aspect of the present invention, the first direction is the average direction of the normal direction of each point in the matched point cloud, and the second direction is perpendicular to the first direction.

[0016] According to one aspect of the present invention, the operation of determining the displacement confidence based on the total number of points within a preset range in a first direction and the total number of points within a preset range in a second direction includes: when the total number of points within the preset range in the first direction is greater than a threshold, the displacement in that direction is credible; when the total number of points within the preset range in the second direction is greater than a threshold, the displacement in that direction is credible.

[0017] According to one aspect of the present invention, the step of matching the point cloud with the map and obtaining the displacement confidence and rotation confidence based on the matching results also includes: clustering the points in the matched point cloud, determining a third direction based on the maximum cluster point cloud, and determining the rotation confidence based on the point cloud distribution in the third direction.

[0018] According to one aspect of the present invention, the third direction is the average direction of the normal direction of each point in the maximum cluster point cloud.

[0019] According to one aspect of the present invention, the operation of determining the third direction based on the maximum cluster point cloud and determining the rotation confidence based on the point cloud distribution in the third direction specifically includes: when the point cloud distribution length in the third direction is greater than a threshold, the corresponding rotation angle is credible.

[0020] According to one aspect of the present invention, the step of obtaining the corrected posture according to the matched point cloud, displacement confidence and rotation confidence comprises:

[0021] Calculating a corrected displacement according to points in the matched point cloud in the first direction and / or the second direction whose displacement is credible;

[0022] Calculating a correction rotation angle according to points in a third direction whose rotation angles are credible in the matched point cloud;

[0023] A correction posture is synthesized according to the correction displacement and the correction rotation angle.

[0024] According to one aspect of the present invention, the operation of calculating the corrective displacement based on the points in the matched point cloud with credible displacements in the first direction and / or the second direction specifically includes: merging the displacement components of each point in the first direction and / or the second direction with credible displacements within a preset range in the direction into the corrective displacement.

[0025] According to one aspect of the present invention, the operation of calculating the correction rotation angle based on the points in the third direction with reliable rotation angles in the matched point cloud specifically includes: merging the angle change of each point in the maximum cluster point cloud in the third direction with reliable rotation angles into the correction rotation angle.

[0026] According to one aspect of the present invention, the step of determining the posture at the current moment based on the predicted posture and the corrected posture specifically includes: determining the weight of the predicted posture based on the measurement accuracy of the odometer, determining the weight of the corrected posture based on the current operating environment, and determining the posture at the current moment based on the predicted posture, the corrected posture and their respective weights.

[0027] The present invention also provides a robot, comprising:

[0028] LiDAR, used to obtain point clouds;

[0029] An odometer, used to obtain the position and posture of the robot; and

[0030] The processor is used to execute the above positioning method to determine the position and posture of the robot at the current moment.

[0031] The present invention determines the current posture of the robot based on the predicted posture of the odometer and the corrected posture of the lidar, and can also assign weights to the predicted posture and the corrected posture to reduce the errors generated in the prediction process and the correction process, thereby avoiding or reducing the occurrence of mismatching and increasing the reliability and stability of the positioning results. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] The drawings constituting a part of the present disclosure are used to provide a further understanding of the present disclosure. The illustrative embodiments of the present disclosure and their descriptions are used to explain the present disclosure and do not constitute an improper limitation on the present disclosure. In the drawings:

[0033] Figure 1a A schematic diagram of a robot according to an embodiment of the present invention is shown;

[0034] Figure 1b Shows Figure 1a Internal module diagram of

[0035] Figure 2 A flow chart of a positioning method for a robot according to an embodiment of the present invention is shown;

[0036] Figure 3a A schematic diagram of robot positioning according to an embodiment of the present invention is shown;

[0037] Figure 3b A schematic diagram of a correction process according to an embodiment of the present invention is shown;

[0038] Figure 4 A schematic diagram of ICP matching according to an embodiment of the present invention is shown;

[0039] Figure 5 A schematic diagram of obtaining a corrected posture according to an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0040] In the following, only some exemplary embodiments are briefly described. As those skilled in the art will appreciate, the described embodiments may be modified in various ways without departing from the spirit or scope of the present invention. Therefore, the drawings and descriptions are considered to be exemplary and non-restrictive in nature.

[0041] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", "clockwise", "counterclockwise" and the like indicate positions or positional relationships based on the positions or positional relationships shown in the accompanying drawings, which are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first" and "second" are only used for descriptive purposes, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, the meaning of "multiple" is two or more, unless otherwise clearly and specifically defined.

[0042] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection, an electrical connection, or can communicate with each other; it can be directly connected, or indirectly connected through an intermediate medium, it can be the internal connection of two elements or the interaction relationship between two elements. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0043] In the present invention, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may include that the first and second features are in direct contact, or may include that the first and second features are not in direct contact but are in contact through another feature between them. Moreover, a first feature being "above", "above" and "above" a second feature includes that the first feature is directly above and obliquely above the second feature, or simply means that the first feature is higher in level than the second feature. A first feature being "below", "below" and "below" a second feature includes that the first feature is directly above and obliquely above the second feature, or simply means that the first feature is lower in level than the second feature.

[0044] The disclosure below provides many different embodiments or examples to realize different structures of the present invention. In order to simplify the disclosure of the present invention, the parts and settings of specific examples are described below. Of course, they are only examples, and the purpose is not to limit the present invention. In addition, the present invention can repeat reference numbers and / or reference letters in different examples, and this repetition is for the purpose of simplicity and clarity, which itself does not indicate the relationship between the various embodiments and / or settings discussed. In addition, the present invention provides various specific examples of processes and materials, but those of ordinary skill in the art can be aware of the application of other processes and / or the use of other materials.

[0045] The present invention provides a positioning method for a robot, the robot includes a laser radar and an odometer, and the positioning method includes: obtaining the posture of the robot through the odometer; predicting the posture at the current moment according to the posture at the previous moment; obtaining the measured point cloud at the current moment through the laser radar; matching the measured point cloud with a map, and obtaining displacement confidence and rotation confidence according to the matching result; obtaining a corrected posture according to the matched point cloud, the displacement confidence and the rotation confidence; determining the posture at the current moment according to the predicted posture and the corrected posture. The present invention determines the current posture of the robot according to the predicted posture of the odometer and the corrected posture of the laser radar, and can also assign weights to the predicted posture and the corrected posture to reduce the errors generated in the prediction process and the correction process, which can avoid or reduce the occurrence of mismatching and increase the reliability and stability of the positioning result.

[0046] The preferred embodiments of the present invention are described below in conjunction with the accompanying drawings. It should be understood that the preferred embodiments described herein are only used to illustrate and explain the present invention, and are not used to limit the present invention.

[0047] Figure 1a A schematic diagram of a robot according to an embodiment of the present invention is shown. Figure 1b Shows Figure 1a The internal module diagram, combined with Figure 1a and Figure 1b The robot comprises: a housing 20 for carrying items, and a mobile chassis 10. The mobile chassis 10 is provided with at least two sets of driving wheels 121, and each set of driving wheels 121 is respectively located on one side of the mobile chassis 10. The robot also comprises a function controller for providing user operation, a bottom controller for map generation and path planning, and an element controller for controlling a mobile unit and an environment detection unit, wherein the element controller is used to control the travel speed of the driving wheels 121. At least one set of driving wheels 121 is used as a left driving wheel, and at the same time, at least one set of driving wheels 121 is used as a right driving wheel, and the left driving wheel and the right driving wheel are located on opposite sides of the chassis 10. Optionally, the robot may also comprise at least two sets of driven wheels, one set of driving wheels corresponding to one set of driven wheels, wherein at least one set of driven wheels is used as a left driven wheel, and at the same time, at least one set of driven wheels is used as a right driven wheel, and the left driven wheel and the right driven wheel are used to assist the left driving wheel and the right driving wheel in driving the housing 20 and the mobile chassis 10 of the robot to move, so as to reduce the load pressure of the driving wheel 120.

[0048] Continue to refer Figure 1a-1b The robot also includes: a laser radar 110, an odometer 120, a memory 130 and a processor 140.

[0049] The laser radar 110 can be set at the slit of the robot shell, so that it is easy to send out laser signals to detect surrounding objects. In a specific embodiment, the laser radar includes a photoelectric receiving array and a laser emitting unit array, so that when the laser radar 110 rotates along the set plane, the photoelectric receiving array can form a scanning cylinder, thereby increasing the scanning area, facilitating the acquisition of the details of the object shape, and avoiding the robot equipment from bumping into the object. In another specific embodiment, the laser radar 110 only includes a single photoelectric receiving unit and a single laser emitting unit. After the laser radar 110 rotates along the set plane, it can measure the object shape of a circle, thereby reducing costs. Optionally, the above-mentioned set plane can be a horizontal plane, which is convenient for the robot to detect objects during the movement. In addition, other set planes, such as vertical planes, can be selected according to user needs, and the present invention does not limit this.

[0050] The odometer 120, for example, is a wheel odometer, which uses the rotation speed values ​​of at least two sets of driving wheels 121 to obtain the linear velocity and angular velocity of the robot, substitutes them into the kinematic model of the mobile robot, and predicts the current posture of the robot, that is, the position and heading angle information. The odometer 120 can also use other odometers, or cooperate with an IMU (inertial measurement unit) module, etc. The present invention does not limit the type of the odometer 120.

[0051] The memory 130 is used for the point cloud map of the current operating environment. In some specific embodiments, the memory 130 includes: a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a static random access memory (SRAM), a portable compact disk read-only memory (CD-ROM), a digital versatile disk (DVD), a memory stick, a floppy disk, a mechanical encoding device, such as a punch card or a protrusion structure in a groove on which instructions are stored, and any suitable combination of the above.

[0052] The processor 140 is used to execute a positioning method to determine the position and posture of the robot at a current moment. Figure 2 A flow chart of a positioning method for a robot according to an embodiment of the present invention is shown. The positioning method includes steps S11-S16, which are specifically as follows:

[0053] In step S11 , the position and posture of the robot is acquired through the odometer 120 .

[0054] The odometer 120, for example, is a wheel odometer, which uses the rotation speed values ​​of at least two sets of driving wheels 121 to obtain the linear velocity and angular velocity of the robot, substitutes them into the kinematic model of the mobile robot, and deduce the position and posture of the robot.

[0055] In step S12, the posture at the current moment is predicted based on the posture at the previous moment.

[0056] The odometer 120 is an effective sensor for the relative positioning of the robot. Its model is determined by the structure and movement mode of the robot, that is, the robot kinematic model. Taking a two-wheel differential robot as an example, the odometer 120 detects the arc of the wheel in a certain period of time based on the photoelectric encoders installed on the left and right sets of driving wheel motors, and then calculates the change in the relative posture of the robot. The present invention does not limit the type and prediction method of the odometer. As long as the posture of the robot at the previous moment is known, the posture of the robot at the current moment can be predicted by the odometer unit, which is within the protection scope of the present invention.

[0057] In step S13, the laser radar 110 is used to obtain the point cloud measured at the current moment.

[0058] Figure 3aA schematic diagram of robot positioning according to an embodiment of the present invention is shown. The robot positioning process based on laser radar and odometer includes the known posture at the previous moment, predicting the posture at the next moment through the odometer 120, corresponding to step S12 of this embodiment, and also includes matching the point cloud information obtained by the laser radar with the map, and correcting the predicted posture, that is, in step S13 of this embodiment, the laser radar 110 is used to obtain the point cloud measured at the current moment, and the corrected posture is obtained in steps S14 and S15, and the correction of the predicted posture is completed in step S16 to obtain the posture at the current moment. Further introduction is given below.

[0059] In step S14, the measured point cloud is matched with the map, and the displacement confidence and the rotation confidence are obtained according to the matching result.

[0060] Figure 3b A schematic diagram of the correction process of an embodiment of the present invention is shown. Before the correction, the measured point cloud and the point cloud map are offset; after the correction, the measured point cloud and the point cloud map are matched. The matching result includes two parts: displacement and rotation. Therefore, after the matching, the displacement confidence and the rotation confidence are obtained according to the matching result. According to the displacement confidence and the rotation confidence, it is determined whether the correction process of the laser radar 110 is credible, and further a credible correction posture is determined.

[0061] According to a preferred embodiment of the present invention, the step of matching the measured point cloud with the map and obtaining the displacement confidence and rotation confidence according to the matching results specifically includes: matching the measured point cloud with the point cloud map of the current operating environment through the ICP algorithm, and obtaining the displacement confidence and rotation confidence according to the matching results.

[0062] The Iterative Closest Point (ICP) algorithm is an algorithm that solves the problem of 3D point cloud registration and can be used for object recognition or pose estimation. Through certain rotation and translation transformations, two sets of point cloud data in different coordinate systems are unified into the same reference coordinate system. The ICP matching results are divided into two parts: displacement and rotation. According to the ICP principle, the displacement confidence is related to the normal angle of the point cloud midpoint in the displacement direction. The smaller the deviation between the normal angle and the displacement direction, the higher the credibility of the displacement correction in this direction. The rotation confidence is related to the range of the points in the continuous matching point cloud. The larger the range of the points in the continuous matching point cloud, the more reliable the rotation result.

[0063] In a specific embodiment, the operation of matching the measured point cloud with the point cloud map of the current operating environment by using the ICP algorithm is as follows:

[0064] The measured point cloud obtained by the laser radar is the target point cloud P to be matched, and the point cloud map of the current operating environment is the source point cloud Q. According to certain constraints, the nearest point (pi, qi) is found, and then the optimal matching parameters R and t are calculated to minimize the error function. The error function E(R, t) is:

[0065]

[0066] Among them, n is the number of nearest neighbor point pairs, pi is a point in the target point cloud P, qi is the nearest point corresponding to pi in the source point cloud Q, R is the rotation matrix, and t is the translation vector.

[0067] Match the target point cloud and the source point cloud using the ICP algorithm:

[0068] (1) Select a point set pi∈P from the target point cloud P;

[0069] (2) Find the corresponding point set qi∈Q in the source point cloud Q, so that ||qi-pi||=min;

[0070] (3) Calculate the rotation matrix R and the translation vector t so that the error function E(R, t) is minimized;

[0071] (4) Use the rotation matrix R and translation vector t to rotate and translate pi to obtain a new corresponding point set pi' = {pi' = Rpi + t, pi∈P};

[0072] (5) The average distance between pi' and the corresponding point set qi is calculated by the following formula:

[0073]

[0074] (6) If the average distance d is less than a preset threshold or greater than a preset maximum number of iterations, the iterative calculation is stopped; otherwise, return to step (2) until the convergence condition is met, thereby completing the matching of the first point cloud with the point cloud map of the calibration room.

[0075] The above is only an exemplary introduction and does not constitute a limitation on the specific method of matching through the ICP algorithm.

[0076] After matching the measured point cloud with the point cloud map of the current operating environment through the ICP algorithm, Figure 4 As shown, the matched point cloud is extracted and processed to obtain the displacement confidence and the rotation confidence.

[0077] According to a preferred embodiment of the present invention, the step of matching the measured point cloud with the map and obtaining the displacement confidence and rotation confidence according to the matching results specifically includes: determining the first direction and the second direction according to the normal direction of each point in the matched point cloud, and determining the displacement confidence according to the total number of points within a preset range of the first direction and the total number of points within a preset range of the second direction.

[0078] According to the ICP principle, the displacement confidence is related to the normal angle of the point in the point cloud in the displacement direction. The smaller the deviation between the normal angle and the displacement direction, the higher the displacement correction confidence in that direction. The normal vector is calculated for each point in the matched point cloud, and the first direction and the second direction are determined according to the direction of the normal vector of each point. The total number of points in the two directions is calculated respectively, and the displacement in which direction is more credible is determined according to the total number of points, or both directions are credible. For example, the more total points a direction has, the more credible the displacement in that direction is.

[0079] According to a preferred embodiment of the present invention, the first direction is the average direction of the normal direction of each point in the matched point cloud, and the second direction is perpendicular to the first direction.

[0080] The normal vector is calculated for each point in the matched point cloud. In a specific embodiment, the calculation process includes: for each point in the matched point cloud, using KD-tree to obtain 8 adjacent points around the point as normal calculation points; performing principal component analysis on the 8 adjacent points, specifically including:

[0081] 1) Calculate the variance matrix cov_matrix of these points;

[0082] 2) Solve the eigenvalues ​​and eigenvectors of the variance matrix con_matrix;

[0083] 3) The eigenvector corresponding to the maximum eigenvalue is the normal direction of the point.

[0084] According to the above embodiment, the normal direction of each point is obtained, the average direction of the normal directions of all points is taken as the first direction, and the direction perpendicular to the direction is taken as the second direction. After the first direction and the second direction are determined, the displacement confidence is determined according to the total number of points within the preset range of the first direction and the total number of points within the preset range of the second direction.

[0085] According to a preferred embodiment of the present invention, the operation of determining the displacement confidence based on the total number of points within a preset range in a first direction and the total number of points within a preset range in a second direction includes: when the total number of points within the preset range in the first direction is greater than a threshold value, the displacement in that direction is credible; when the total number of points within the preset range in the second direction is greater than a threshold value, the displacement in that direction is credible.

[0086] In a specific embodiment, the preset range is 45°, the threshold is N, and when the total number of points in the matching point cloud whose normal direction has an acute angle with the first direction less than 45° is greater than N, the displacement in the first direction is credible; when the total number of points in the matching point cloud whose normal direction has an acute angle with the second direction less than 45° is greater than N, the displacement in the second direction is credible.

[0087] In a specific embodiment, the process of determining the threshold N is as follows:

[0088] Build different types of scenes in the test environment, such as long corridors, rooms with special materials, and cluttered places, to ensure that the threshold N is universal in any known scene;

[0089] Place a robot equipped with a high-precision odometer (e.g., accuracy <1 / 1000) and a high-precision IMU in the test environment, and manually set the initial position of the robot to ensure that the initial position is correct;

[0090] Control the robot to walk a certain distance and angle. It can be assumed that the odometer and IMU predicted posture are completely correct during the walking period.

[0091] Perform ICP algorithm matching with predicted posture + certain posture deviation to check whether the matched posture is consistent with the predicted posture. For example, when the deviation value between the matched posture and the predicted posture is less than a certain range, it means that the correction result matched by ICP algorithm is correct.

[0092] The threshold N is continuously adjusted and the ICP algorithm matching is repeated to find the posture deviation that is consistent with the predicted posture after matching. The threshold N corresponding to the posture deviation is the universal threshold, which can ensure that the correction results matched by the ICP algorithm in any known scene are correct.

[0093] When the total number of points within the preset range of the first direction is greater than the threshold value N, the displacement in this direction is credible; when the total number of points within the preset range of the second direction is greater than the threshold value N, the displacement in this direction is credible, thereby determining the displacement confidence. According to the ICP principle, the matching results are divided into two parts: displacement and rotation, so the rotation confidence also needs to be determined. The rotation confidence is related to the range of the points in the continuous matching point cloud. The larger the range of the points in the continuous matching point cloud, the more reliable the generated rotation result. This is further described below.

[0094] According to a preferred embodiment of the present invention, the step of matching the point cloud with the map and obtaining the displacement confidence and rotation confidence based on the matching results also includes: clustering the points in the matched point cloud, determining a third direction based on the maximum cluster point cloud, and determining the rotation confidence based on the point cloud distribution in the third direction.

[0095] According to a preferred embodiment of the present invention, the third direction is the average direction of the normal direction of each point in the maximum cluster point cloud.

[0096] Continue to refer Figure 4 , cluster the points in the matched point cloud, calculate the normal direction of each point in the largest cluster, and then take the average direction of all normal directions as the third direction. According to the point cloud distribution in the third direction, determine the rotation confidence.

[0097] In a specific embodiment, the process of calculating the normal direction of each point in the largest cluster includes: for each point in the largest cluster, using KD-tree to obtain 8 adjacent points around the point as normal calculation points; performing principal component analysis on the 8 adjacent points, specifically including:

[0098] 1) Calculate the variance matrix cov_matrix of these points;

[0099] 2) Solve the eigenvalues ​​and eigenvectors of the variance matrix con_matrix;

[0100] 3) The eigenvector corresponding to the maximum eigenvalue is the normal direction of the point.

[0101] The normal direction of each point is obtained according to the above method, and the average direction of the normal directions of all points is taken as the third direction.

[0102] According to a preferred embodiment of the present invention, the operation of determining the third direction based on the maximum cluster point cloud and determining the rotation confidence based on the point cloud distribution in the third direction specifically includes: when the point cloud distribution length in the third direction is greater than a threshold, the corresponding rotation angle is credible.

[0103] In a specific embodiment, the point cloud distribution length is L, the threshold is 0.3 m, and when L is greater than 0.3 m, the corrected angle variation is considered correct, that is, the corresponding rotation angle is credible.

[0104] In step S15, the corrected posture is obtained according to the matched point cloud, the displacement confidence and the rotation confidence.

[0105] According to a preferred embodiment of the present invention, the step of obtaining the corrected posture according to the matched point cloud, displacement confidence and rotation confidence specifically comprises:

[0106] Calculating a corrected displacement according to points in the matched point cloud in the first direction and / or the second direction whose displacement is credible;

[0107] Calculating a correction rotation angle according to points in a third direction whose rotation angles are credible in the matched point cloud;

[0108] A correction posture is synthesized according to the correction displacement and the correction rotation angle.

[0109] Specifically, according to the normal direction of each point in the matched point cloud, the first direction and the second direction are determined, and the displacement confidence is determined according to the total number of points within the preset range of the first direction and the total number of points within the preset range of the second direction. If the displacement in the first direction is credible, the displacement in this direction is retained, otherwise, the displacement in this direction is discarded; if the displacement in the second direction is credible, the displacement in this direction is retained, otherwise, the displacement in this direction is discarded. Finally, the retained displacement is synthesized into the corrected displacement. The points in the matched point cloud are clustered, and the third direction is determined according to the maximum clustered point cloud. According to the point cloud distribution in the third direction, the rotation confidence is determined. If the point cloud distribution length in the third direction is greater than the threshold, the corresponding rotation angle is credible and retained, otherwise, it should be discarded. Finally, the retained rotation angle is synthesized into the final corrected rotation angle. According to the corrected displacement and the corrected rotation angle, the corrected posture is synthesized.

[0110] According to a preferred embodiment of the present invention, the operation of calculating the corrective displacement based on the points in the matched point cloud with credible displacements in the first direction and / or the second direction specifically includes: merging the displacement components of each point in the first direction and / or the second direction with credible displacements within a preset range in the direction into the corrective displacement.

[0111] For example, if the displacement in the first direction is credible, the points in the preset range in this direction are retained, otherwise, the displacement in this direction is discarded; if the displacement in the second direction is credible, the points in the preset range in this direction are retained, otherwise, the displacement in this direction is discarded. Finally, the displacement components of each point in the preset range retained in the two directions are combined into the corrected displacement.

[0112] According to a preferred embodiment of the present invention, the operation of calculating the correction rotation angle based on the points in the third direction with credible rotation angles in the matched point cloud specifically includes: merging the angle change of each point in the maximum cluster point cloud in the third direction with credible rotation angles into the correction rotation angle.

[0113] For example, if the point cloud distribution length in the third direction is greater than the threshold, the corresponding rotation angle is credible and the points within the preset range are retained. Otherwise, the points within the preset range should be discarded. Finally, the angle change of each retained point is combined into the corrected rotation angle.

[0114] The above describes how to determine the corrected posture by using the displacement confidence and the rotation confidence, which is further described below through a specific embodiment.

[0115] Figure 5FIG. 1 is a schematic diagram of obtaining a corrected posture according to an embodiment of the present invention. The irregular frame represents the current operating environment, the solid black dots represent the point cloud obtained by the laser radar 110, and the arrows next to each solid dot represent the normal direction. The corrected posture C can be obtained by (x C ,y C ,θ C ), that is, the correction posture can be decomposed into the x-axis correction factor, the y-axis correction factor and the angle θ correction factor. The specific way to perform point cloud matching and obtain the correction posture through the ICP algorithm is to obtain these three correction factors. Correspondingly, the posture A at the previous moment can be expressed as (x A ,y A ,θ A ), the predicted pose B can be expressed as (x B ,y B ,θ B ).

[0116] For example, in a long corridor, Figure 5 For the left side wall, the normal direction of the points in the measured point cloud all corresponds to the first direction, and there is no component in the second direction. The component in the first direction is retained, and due to the special terrain of the long corridor, the point cloud distribution length is greater than the threshold L, and the rotation angle is unreliable. Therefore, the final corrected pose is (x C , 0,0).

[0117] For example, when not in a long corridor, Figure 5 For the single-sided wall on the upper left side, the normal direction of the points in the measured point cloud has both the first direction component and the second direction component, and the total number of points in both directions is greater than the threshold N, then both directions are retained, and further determination is made as to whether the point cloud distribution length is greater than the threshold M. If it is greater than the threshold M, the rotation angle is unreliable, and the final corrected pose is (x C ,y c , 0). The robot positioning process based on laser radar and odometer is divided into three steps: first, the posture at the previous moment is known, and the posture at the next moment is predicted by the odometer 120, which corresponds to step S12 of this embodiment; second, the predicted posture is corrected by matching the laser information with the map, which corresponds to step S15 of this embodiment; third, the posture at the current moment is determined according to the predicted posture of the odometer 120 and the corrected posture of the laser radar 110, which corresponds to step S16 of this embodiment. The following is further introduced.

[0118] In step S16, the current posture is determined based on the predicted posture and the corrected posture.

[0119] Continue to refer Figure 3a, the posture at the previous moment is known, and the posture at the current moment is predicted by the odometer 120 to obtain the predicted posture. The measured point cloud is obtained by the laser radar 110, and the corrected posture is obtained by matching according to the ICP algorithm. The posture at the current moment is determined according to the predicted posture and the corrected posture.

[0120] According to a preferred embodiment of the present invention, the step of determining the posture at the current moment based on the predicted posture and the corrected posture specifically includes: determining the weight of the predicted posture based on the measurement accuracy of the odometer, determining the weight of the corrected posture based on the current operating environment, and determining the posture at the current moment based on the predicted posture, the corrected posture and their respective weights.

[0121] During the positioning process, the predicted posture obtained by the odometer 120 and the corrected posture obtained by the lidar 110 may include errors. When determining the final result, whether to trust the posture predicted by the odometer 120 or the corrected posture of the lidar more can be determined by assigning corresponding weights according to the size of the errors between the two. The weight of the odometer 120 is related to the measurement accuracy of the odometer. The higher the measurement accuracy, the greater the weight. The weight of the lidar 110 is related to the current operating environment. Different operating environments, such as long corridors, rooms with special materials, and cluttered places, have different weights. Different types of scenes can be built and obtained through multiple measurement data.

[0122] In summary, the present invention determines the current posture of the robot based on the predicted posture of the odometer and the corrected posture of the lidar, and can also assign weights to the predicted posture and the corrected posture to reduce the errors generated in the prediction process and the correction process, thereby avoiding or reducing the occurrence of mismatching and increasing the reliability and stability of the positioning results.

[0123] This specification provides method operation steps as described in the embodiments or flowcharts, but more or fewer operation steps may be included based on routine or non-creative work. The order of steps listed in the embodiments is only one way of executing the order of many steps and does not represent the only execution order. When the actual system or device product is executed, it can be executed in the order or in parallel according to the method shown in the embodiments or flowcharts.

[0124] The present invention also relates to a robot, Figure 1a-1b , the robot comprises:

[0125] A laser radar 110, used to obtain a point cloud;

[0126] an odometer 120, used to obtain the position and posture of the robot; and

[0127] The processor 140 is used to execute the above positioning method to determine the position and posture of the robot at the current moment.

[0128] The present invention determines the current posture of the robot based on the predicted posture of the odometer and the corrected posture of the lidar, and can also assign weights to the predicted posture and the corrected posture to reduce the errors generated in the prediction process and the correction process, thereby avoiding or reducing the occurrence of mismatching and increasing the reliability and stability of the positioning results.

[0129] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A positioning method for a robot, the robot comprising a laser radar and an odometer, the positioning method comprising: Acquiring the position and posture of the robot through the odometer; Predict the current position based on the previous position; Acquire the point cloud measured at the current moment through the laser radar; Matching the measured point cloud with the map, and obtaining displacement confidence and rotation confidence according to the matching result; Obtaining a corrected posture according to the matched point cloud, the displacement confidence and the rotation confidence; Determine the current posture according to the predicted posture and the corrected posture; The step of matching the measured point cloud with the map and obtaining the displacement confidence and the rotation confidence according to the matching result specifically includes: matching the measured point cloud with the point cloud map of the current operating environment through the ICP algorithm, and obtaining the displacement confidence and the rotation confidence according to the matching result; The step of matching the measured point cloud with the map and obtaining displacement confidence and rotation confidence based on the matching results also includes: determining the first direction and the second direction based on the normal direction of each point in the matched point cloud, and determining the displacement confidence based on the total number of points within a preset range of the first direction and the total number of points within a preset range of the second direction.

2. The positioning method according to claim 1, wherein the first direction is the average direction of the normal direction of each point in the matched point cloud, and the second direction is perpendicular to the first direction.

3. The positioning method according to claim 1, wherein the operation of determining the displacement confidence according to the total number of points within the preset range in the first direction and the total number of points within the preset range in the second direction comprises: When the total number of points within the preset range in the first direction is greater than the threshold, the displacement in this direction is credible; When the total number of points within the preset range in the second direction is greater than a threshold, the displacement in this direction is credible.

4. The positioning method according to claim 3, wherein the step of matching the measured point cloud with the map and obtaining the displacement confidence and the rotation confidence according to the matching result further comprises: The points in the matched point cloud are clustered, a third direction is determined according to the maximum clustered point cloud, and a rotation confidence is determined according to the point cloud distribution in the third direction. 5 . The positioning method according to claim 4 , wherein the third direction is the average direction of the normal direction of each point in the maximum cluster point cloud.

6. The positioning method according to claim 4, wherein the operations of determining the third direction according to the maximum cluster point cloud and determining the rotation confidence according to the point cloud distribution of the third direction specifically include: When the point cloud distribution length in the third direction is greater than a threshold, the corresponding rotation angle is credible.

7. The positioning method according to claim 6, wherein the step of obtaining the corrected posture according to the matched point cloud, displacement confidence and rotation confidence comprises: Calculating a corrected displacement according to points in the matched point cloud in the first direction and / or the second direction whose displacement is credible; Calculating a correction rotation angle according to points in a third direction whose rotation angles are credible in the matched point cloud; A correction posture is synthesized according to the correction displacement and the correction rotation angle.

8. The positioning method according to claim 7, wherein the operation of calculating the corrected displacement according to the points in the first direction and / or the second direction in the matched point cloud whose displacement is credible specifically comprises: The displacement components of each point in the first direction and / or the second direction with credible displacement within a preset range in the direction are combined into the corrected displacement.

9. The positioning method according to claim 7, wherein the operation of calculating the corrected rotation angle according to the points in the third direction with credible rotation angles in the matched point cloud specifically comprises: The angle change of each point in the largest cluster point cloud in the third direction with reliable rotation angle is merged into the corrected rotation angle.

10. The positioning method according to any one of claims 1 to 9, wherein the step of determining the current posture according to the predicted posture and the corrected posture specifically comprises: The weight of the predicted posture is determined according to the measurement accuracy of the odometer, the weight of the corrected posture is determined according to the current operating environment, and the posture at the current moment is determined according to the predicted posture, the corrected posture and their respective weights.

11. A robot comprising: LiDAR, used to obtain point clouds; An odometer, used to obtain the position and posture of the robot; and A processor, configured to execute the positioning method according to any one of claims 1 to 10, so as to determine the position and posture of the robot at a current moment.

Images