Robot positioning method, device, storage medium and robot
By combining a multi-sensor fusion method of foot odometry and single-line LiDAR, and utilizing the iterative nearest point algorithm and transformation matrix calculation, the problem of insufficient positioning accuracy of foot odometry was solved, thereby improving the accuracy of robot positioning.
Patent Information
- Application Number
- CN202110951747.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-08-18
- Publication Date
- 2024-11-01
- Estimated Expiration
- 2041-08-18
AI Technical Summary
Foot-based odometry is affected by air resistance and ground friction in robot localization, resulting in insufficient positioning accuracy.
By employing a multi-sensor fusion approach, combining foot odometry and single-line LiDAR, the robot's real-time positioning is corrected by acquiring pose information and point cloud data and utilizing iterative nearest-point algorithm and transformation matrix calculation.
It improves the accuracy of robot localization by correcting the desired position in the world coordinate system to obtain a more accurate actual position.
Smart Images

Figure CN113671523B_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of computer technology, and in particular to a robot positioning method, device, storage medium and robot. Background Art
[0002] With the development of artificial intelligence technology, the research and development of mobile robots is very rapid. When the robot is performing a task, in order to ensure the position of the robot or prevent the robot from being lost, the robot needs to be positioned in real time.
[0003] When a legged robot performs real-time positioning, it uses a foot-based odometer for real-time positioning. However, when the foot-based odometer is used, the robot is affected by factors such as air resistance and ground friction, and the real-time positioning obtained has a large error and the positioning accuracy is insufficient. Summary of the invention
[0004] The embodiments of the present application provide a robot positioning method, device, storage medium and robot, which use multi-sensor fusion to correct the desired position of the robot in real time, thereby improving the accuracy of the robot's real-time positioning.
[0005] The technical solution is as follows:
[0006] In a first aspect, an embodiment of the present application provides a robot positioning method, which is applied to a robot, wherein the robot includes a foot odometer and a single-line laser radar, and the method includes:
[0007] Obtaining the posture information collected by the foot odometer at the previous timestamp, and obtaining the point cloud data collected by the single-line laser radar at the current timestamp;
[0008] Calculating the expected position of the current timestamp in the world coordinate system based on the pose information collected at the previous timestamp;
[0009] Mapping the point cloud data collected at the current timestamp and the expected position of the current timestamp to the world coordinate system to obtain a transformation matrix;
[0010] The actual position of the current timestamp in the world coordinate system is calculated based on the transformation matrix and the expected position.
[0011] In a second aspect, an embodiment of the present application provides a robot positioning device, which is applied to a robot, wherein the robot includes a foot odometer and a single-line laser radar, and the device includes:
[0012] A data acquisition module, used to acquire the posture information collected by the foot odometer at the previous timestamp, and to acquire the point cloud data collected by the single-line laser radar at the current timestamp;
[0013] An expected position calculation module, used to calculate the expected position of the current timestamp in the world coordinate system based on the posture information collected at the previous timestamp;
[0014] A transformation matrix calculation module, used to map the point cloud data collected at the current timestamp and the expected position of the current timestamp to the world coordinate system to obtain a transformation matrix;
[0015] The actual position calculation module is used to calculate the actual position of the current timestamp in the world coordinate system based on the transformation matrix and the expected position.
[0016] In a third aspect, an embodiment of the present application provides a computer storage medium, wherein the computer storage medium stores a plurality of instructions, wherein the instructions are suitable for being loaded by a processor and executing the above-mentioned method steps.
[0017] In a fourth aspect, an embodiment of the present application provides a robot, which may include: a processor and a memory;
[0018] The memory stores a computer program, and the computer program is suitable for being loaded by the processor and executing the above method steps.
[0019] The beneficial effects brought about by the technical solutions provided by some embodiments of the present application include at least:
[0020] In an embodiment of the present application, a foot-type odometer and a single-line laser radar are installed on the robot, and the posture information collected by the foot-type odometer at the previous timestamp is obtained, and the point cloud data collected by the single-line laser radar at the current timestamp is obtained. The expected position of the current timestamp in the world coordinate system is calculated based on the posture information collected at the previous timestamp, and the point cloud data collected at the current timestamp and the expected position of the current timestamp are mapped to the world coordinate system to obtain a transformation matrix, and the actual position of the current timestamp in the world coordinate system is calculated based on the transformation matrix and the expected position. This method is used to correct the expected position of the current timestamp in the world coordinate system, and the corrected expected position of the current timestamp is obtained, so that the actual position of the current timestamp can be obtained, and the real-time positioning of the robot is completed, thereby improving the accuracy of the robot positioning. BRIEF DESCRIPTION OF THE DRAWINGS
[0021] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0022] Figure 1 It is a flowchart of a robot positioning method provided in an embodiment of the present application;
[0023] Figure 2 is a structural schematic diagram of a robot positioning device provided in an embodiment of the present application;
[0024] Figure 3 is a structural schematic diagram of a robot positioning device provided in an embodiment of the present application;
[0025] Figure 4 It is a flowchart of a robot positioning method provided in an embodiment of the present application;
[0026] Figure 5 is a structural schematic diagram of a robot positioning device provided in an embodiment of the present application;
[0027] Figure 6 is a structural diagram of an expected position calculation module provided in an embodiment of the present application;
[0028] Figure 7 It is a structural diagram of a matrix transformation calculation module provided in an embodiment of the present application;
[0029] Figure 8 It is a schematic diagram of the structure of a robot provided in an embodiment of the present application. DETAILED DESCRIPTION
[0030] In order to make the objectives, technical solutions and advantages of the present application more clear, the embodiments of the present application will be further described in detail below with reference to the accompanying drawings.
[0031] When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present application. Instead, they are only examples of devices and methods consistent with some aspects of the present application as detailed in the attached claims.
[0032] In the description of the present application, it should be understood that the terms "first", "second", etc. are only used for descriptive purposes and should not be understood as indicating or implying relative importance. For those of ordinary skill in the art, the specific meanings of the above terms in the present application can be understood according to the specific circumstances. In addition, in the description of the present application, unless otherwise specified, "multiple" refers to two or more. "And / or" describes the association relationship of associated objects, indicating that three relationships may exist. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. The character " / " generally indicates that the previously associated objects are in an "or" relationship.
[0033] The present application is described in detail below with reference to specific embodiments.
[0034] Among them, for wheeled robots, the wheeled odometers they carry as positioning devices for wheeled robots can only collect posture information such as the X-axis direction, Y-axis direction, and yaw angle (Yaw) in the coordinate system based on the wheeled odometer. Compared with wheeled odometers, foot-based odometers can collect posture information such as the X-axis direction, Y-axis direction, Z-axis direction, yaw angle (Yaw), roll angle (Roll), and pitch angle (Pitch) in the coordinate system based on the foot-based odometer, and the collected information is more complete. However, when foot-based odometers are used, the robot is affected by factors such as air resistance and ground friction, and the real-time positioning obtained has large errors and insufficient positioning accuracy.
[0035] Based on this, a robot positioning method in an embodiment of the present application is applied to a robot, wherein the robot includes a foot odometer and a single-line laser radar.
[0036] Among them, the foot-type odometer is the positioning device of the legged robot, which can collect the position and posture information of the legged robot.
[0037] A single-line laser radar refers to a laser radar whose laser source emits a single-line beam. The single-line laser radar transmits a detection signal (laser beam) to objects around the robot, and then compares the reflected signal (echo) with the transmitted detection signal. After appropriate processing, it can obtain information such as direction, altitude, speed, posture, and even shape.
[0038] The following will be combined with the attached Figure 1 -Attached Figure 8 , a robot positioning method provided in an embodiment of the present application is introduced in detail. The method can be implemented by a computer program and can be run on a robot positioning device based on a von Neumann system. The computer program can be integrated into an application or run as an independent tool application.
[0039] See also Figure 1 , which is a schematic diagram of a flow chart of a robot positioning method provided in an embodiment of the present application. Figure 1 As shown, the method of the embodiment of the present application may include the following steps:
[0040] S101, obtaining the posture information collected by the foot odometer at the previous timestamp, and obtaining the point cloud data collected by the single-line laser radar at the current timestamp;
[0041] Timestamp is data generated by digital signature technology, and the signature object includes original file information, signature parameters, signature time, etc. Therefore, different timestamps represent different moments.
[0042] The foot odometer can be used to collect pose information at different timestamps, and the single-line laser radar can be used to collect point cloud data at different timestamps. It should be noted that the timestamps of the two are one-to-one corresponding.
[0043] Among them, posture refers to position and posture, and posture information is the specific value of the parameters describing the posture, such as Figure 2 As shown in the figure, the posture information includes the values of the robot's X-axis direction, Y-axis direction, Z-axis direction, pitch angle (Pitch), yaw angle (Yaw), and roll angle (Roll) in the coordinate system based on the foot odometer.
[0044] Among them, the X-axis represents the horizontal coordinate in the three-dimensional space, the Y-axis represents the vertical coordinate in the three-dimensional space, and the Z-axis represents the vertical coordinate in the three-dimensional space.
[0045] The pitch angle is the direction of rotation around the X-axis in three-dimensional space. The value of the pitch angle can represent the angle at which the robot or the robot's head deflects to the up and down directions with the robot as the center; the yaw angle is the direction of rotation around the Y-axis in three-dimensional space. The value of the pitch angle can represent the angle at which the robot or the robot's head deflects to the left and right directions with the robot as the center; the roll angle is the direction of rotation around the Z-axis in three-dimensional space. It can represent the angle at which the robot or the robot's head tilts to the left and right directions with the robot as the center.
[0046] Point cloud data refers to the echo data generated based on the reflected laser when a laser beam from a single-line laser radar hits the surface of an object, which contains information such as direction, distance, speed, posture, and even shape.
[0047] The posture information collected by the foot-type odometer and the point cloud data collected by the single-line lidar both contain the positioning information of the legged robot.
[0048] During the walking process of the legged robot, the foot odometer collects posture information at each timestamp, and the single-line laser radar collects point cloud data at each timestamp. The posture information collected by the foot odometer at the previous timestamp can be obtained by taking the current timestamp of the legged robot during walking as a reference, obtaining the posture information collected at the timestamp before the current timestamp, and at the same time, based on the current timestamp, obtaining the point cloud data collected by the single-line laser radar at the current timestamp. The foot odometer collects posture information at the current timestamp, and the single-line laser radar collects point cloud data at the current timestamp. The position of the posture information collected by the foot odometer at the current timestamp is the same as or close to the position of the point cloud data collected by the single-line laser radar at the current timestamp.
[0049] It should be noted that the time interval between the previous timestamp and the current timestamp is preset, such as 1 second.
[0050] S102, calculating the expected position of the current timestamp in the world coordinate system based on the posture information collected at the previous timestamp;
[0051] The expected position at the current timestamp is the ideal position of the robot in the world coordinate system calculated at the current timestamp based on the previous timestamp, that is, the estimated position.
[0052] Specifically, the expected position of the current timestamp is obtained by adding the value corresponding to the position in the pose information collected at the previous timestamp to the position offset. Figure 3 As shown, position A represents the position in the posture information of the previous timestamp, and position B represents the expected position of the current timestamp. The expected position of the current timestamp can be calculated from the position in the posture information of the previous timestamp and the position offset.
[0053] The position offset is the fixed displacement value of the robot between two timestamps under an ideal state, which can be calculated from the posture information of a timestamp selected at random during the robot's walking process and the previous timestamp of the selected timestamp. For example, in an embodiment of the present application, it can be calculated from the posture information collected by the foot odometer at the previous timestamp and the posture information collected by the foot odometer at the current timestamp.
[0054] S103, mapping the point cloud data collected at the current timestamp and the expected position of the current timestamp to the world coordinate system to obtain a transformation matrix;
[0055] The iterative closest point (ICP) algorithm is used to map the point cloud data collected at the current timestamp and the expected position of the current timestamp to the world coordinate system.
[0056] The ICP algorithm is based on data registration and uses the nearest point search method to achieve point set to point set registration.
[0057] By mapping the point cloud data collected at the current timestamp and the expected position at the current timestamp to the world coordinate system, the point cloud data collected at the current timestamp and the expected position at the current timestamp correspond one to one in the world coordinate system, thereby achieving the purpose of information fusion.
[0058] After mapping the point cloud data collected at the current timestamp and the expected position of the current timestamp to the world coordinate system, the nearest neighboring point of each point cloud in the point cloud data is searched in the world coordinate system.
[0059] The nearest neighboring point is the coordinate point that is mapped from the expected position of the current timestamp to the coordinate point of the world coordinate system and is closest to the coordinate point corresponding to the position in the point cloud data.
[0060] After searching for the nearest neighboring point of each point cloud in the point cloud data, the transformation matrix between each point cloud in the point cloud data and the nearest neighboring point is calculated.
[0061] S104: Calculate the actual position of the current timestamp in the world coordinate system based on the transformation matrix and the expected position.
[0062] The expected position of the current timestamp is the result of estimating the position of the robot at the current timestamp, not the actual position of the robot. Since the error of the pose information of any timestamp collected by the foot odometer is larger than that of the point cloud data of any timestamp collected by the single-line laser radar, that is, the point cloud data collected by the single-line laser radar has a higher positioning accuracy than the pose information collected by the foot odometer, and is close to the actual position of the robot. The pose information of the previous timestamp collected by the foot odometer plus the fixed position offset obtains the expected position of the current timestamp, which has a large error compared with the actual position of the robot. Therefore, the expected position of the current timestamp is corrected based on the point cloud data collected by the single-line laser radar. The transformation matrix expresses the position difference between the position in the point cloud data of the current timestamp collected by the single-line laser radar and the expected position of the current timestamp. Therefore, the expected position of the current timestamp is multiplied by the transformation matrix, and the calculated result is used to correct the expected position of the current timestamp to obtain the actual position of the current timestamp in the world coordinate system.
[0063] In an embodiment of the present application, the posture information collected by the foot odometer at the previous timestamp is obtained, and the point cloud data collected by the single-line laser radar at the current timestamp is obtained. The expected position of the current timestamp in the world coordinate system is calculated based on the posture information collected at the previous timestamp. The point cloud data collected at the current timestamp and the expected position of the current timestamp are mapped to the world coordinate system to obtain a transformation matrix. The actual position of the current timestamp in the world coordinate system is calculated based on the transformation matrix and the expected position. This method is used to correct the expected position of the current timestamp in the world coordinate system, and the corrected expected position of the current timestamp is obtained. The actual position of the current timestamp can be obtained, and the real-time positioning of the robot is completed, thereby improving the accuracy of the robot positioning.
[0064] See also Figure 4 , is a flow chart of a robot positioning method provided in an embodiment of the present application. Figure 4 As shown, the robot positioning method may include the following steps:
[0065] S201, obtaining the posture information collected by the foot odometer at the previous timestamp, and obtaining the point cloud data collected by the single-line laser radar at the current timestamp;
[0066] The process of obtaining the posture information collected by the foot odometer at the previous timestamp and obtaining the point cloud data collected by the single-line laser radar at the current timestamp is the same as S101.
[0067] Since both the foot odometer and the single-line laser radar are devices that can realize the positioning of the robot, at the same timestamp, the position in the posture information collected by the foot odometer and the position in the point cloud data collected by the single-line laser radar express similar positioning, and both can be used as data to determine the current positioning of the robot.
[0068] S202, obtaining the position and posture information collected by the foot odometer at the current timestamp;
[0069] Based on the posture information collected at the previous timestamp and the posture information collected at the current timestamp, obtaining a position offset of the foot odometer between the previous timestamp and the current timestamp;
[0070] The current timestamp can be the first timestamp or any other timestamp selected by the robot during walking. The current timestamp in the pose information collected by the foot odometer at the current timestamp and the current timestamp in the point cloud data of the current timestamp collected by the single-line laser radar are the same timestamp.
[0071] The position offset can be obtained based on the pose information of two adjacent timestamps. For example, when the pose information collected at the previous timestamp is T1 and the pose information collected at the current timestamp is T2, the position offset between the previous timestamp and the current timestamp is calculated as follows:
[0072]
[0073] S203, calculating the expected position of the current timestamp in the world coordinate system based on the posture information collected at the previous timestamp and the position offset;
[0074] Specifically, the pose information collected at the previous timestamp is added to the position offset between the previous timestamp and the current timestamp to obtain the expected position of the current timestamp in the world coordinate system.
[0075] The expected position at the current timestamp is the result of estimating the ideal position of the robot at the current timestamp. It is not the position in the pose information at the current timestamp collected by the foot odometer, nor is it the actual position of the robot at the current timestamp.
[0076] S204, mapping the point cloud data collected at the current timestamp and the expected position of the current timestamp to the world coordinate system; searching the world coordinate system for the nearest neighboring point of each point cloud in the point cloud data;
[0077] The point cloud data collected at the current timestamp and the expected position of the current timestamp are mapped to the world coordinate system to generate a grid map, wherein the coordinate system used by the grid map is the world coordinate system.
[0078] The raster map, also known as the raster image, refers to an image that has been discretized in space and brightness. The raster map obtained by mapping the point cloud data collected at the current timestamp and the expected position at the current timestamp to the world coordinate system contains the specific coordinate information of the position of the point cloud data collected at the current timestamp and the expected position at the current timestamp in the world coordinate system.
[0079] The specific process of building a raster map is to perform loop detection on the point cloud data collected at the current timestamp and the expected position of the current timestamp. There are many ways to perform loop detection, such as the SLAM (Simultaneous Localization and Mapping) algorithm based on graph optimization, the bag-of-words model, similarity calculation, deep learning, etc. When a loop is detected, the point cloud data in the world coordinate system and the motion trajectory generated by the pose information can be optimized by the singular value decomposition (SVD) algorithm or nonlinear optimization method to obtain the optimized motion trajectory. The optimization process of the SVD algorithm is used as an example to illustrate the following.
[0080] Given two point sets (e.g. point cloud data collected by a single-line lidar and the expected position at the current timestamp):
[0081] X={X1,X2,X3,X4,X5…-Xnx}
[0082] P={P1,P2,P3,P4,P5…-Pnx}
[0083] Find a Euclidean transformation R,t such that:
[0084] Solve R, t to minimize the error function:
[0085]
[0086] Compute the centroid positions of two sets of points:
[0087]
[0088]
[0089] Compute the de-centroided coordinates of each point:
[0090] X′={Xi-ux}={Xi′}
[0091] X′={Xi-ux}={Xi′}
[0092] Derive the objective function:
[0093]
[0094] 2 (Xi-ux-R (pi-up)) T The sum of the integrals of (ux-Rup-t) is 0, so the above formula can be simplified to:
[0095]
[0096] Therefore, the optimization objective function becomes:
[0097]
[0098] Observe this optimization objective function, where ||(Xi-Ux-R(pi-up))|| 2 It is only related to the rotation matrix R, while ||(Ux-Rup-t)|| 2 Related to R and t, if we only need R, we can set the second term to 0 and obtain the translation matrix t, so the actual optimization objective function is as follows:
[0099]
[0100] can be simplified to:
[0101]
[0102]
[0103] make After SVD decomposition, in The singular values form a diagonal matrix, the diagonal elements are arranged from large to small, and U and V are diagonal matrices. When W is full rank, the rotation matrix R is:
[0104] R=UV T
[0105] Then the translation matrix t is:
[0106] t=ux-Rup
[0107] Generate a grid map based on the optimized motion trajectory.
[0108] Based on the generated grid map, search for the nearest neighboring point of each point cloud in the point cloud data.
[0109] Compared with the pose information collected based on the foot odometer, the point cloud data collected by the single-line laser radar has higher accuracy. When the point cloud data collected at the current timestamp is mapped to the world coordinate system, there is a certain position deviation from the expected position of the current timestamp, and it is not located at the same coordinate point in the world coordinate system. Therefore, the nearest neighboring points of each point cloud in the point cloud data collected by the single-line laser radar are searched so that the expected position of the current timestamp in the world coordinate system can be corrected based on the point cloud data collected by the single-line laser radar.
[0110] Specifically, the generated raster map maps the point cloud data collected at the current timestamp to the world coordinate system, obtains the coordinate points corresponding to each point cloud in the point cloud data in the world coordinate system based on the position of each point cloud in the point cloud data, and maps the expected position of the current timestamp to the world coordinate system, obtains the coordinate points corresponding to the expected position of the current timestamp in the world coordinate system based on the expected position of the current timestamp, searches for the coordinate points corresponding to the expected position of the current timestamp in the world coordinate system that are closest to the coordinate points corresponding to each point cloud in the world coordinate system, and the coordinate points corresponding to the expected position of the current timestamp in the world coordinate system are the nearest neighboring points. Calculate the transformation matrix between the coordinate points corresponding to each point cloud in the point cloud data and the nearest neighboring points of each point cloud in the point cloud data.
[0111] S205, calculating a transformation matrix between each point cloud in the point cloud data and the nearest adjacent point;
[0112] The transformation matrix between each point cloud and the nearest neighboring point in the point cloud data can be calculated by using a Nearest K Search algorithm, wherein the Nearest K Search is an algorithm in the open source library PCL.
[0113] The transformation matrix represents the positional difference between each point cloud and its nearest neighboring point in the point cloud data. Therefore, based on the transformation matrix, we can understand the degree of coordinate offset between each point cloud and its nearest neighboring point in the point cloud data, and the error of the posture information collected by the foot odometer.
[0114] S206: Multiply the expected position by the transformation matrix to obtain the actual position of the current timestamp in the world coordinate system.
[0115] The expected position of the current timestamp is multiplied by the transformation matrix, and the calculated result is used to correct the expected position of the current timestamp to obtain the actual position of the current timestamp in the world coordinate system, thereby completing the real-time positioning of the robot.
[0116] Specifically, at the next timestamp, the posture information collected by the foot odometer at the current timestamp is obtained, and the point cloud data collected by the single-line laser radar at the next timestamp is obtained. Based on the posture information collected at the current timestamp and the position offset, the expected position of the next timestamp in the world coordinate system is calculated, and the expected position of the next timestamp in the world coordinate system is corrected to obtain the actual position of the next timestamp in the world coordinate system. This method is used at each timestamp to correct the expected position of the timestamp of the legged robot in the world coordinate system, thereby completing the real-time positioning of the robot during walking.
[0117] In an embodiment of the present application, the posture information collected by the foot odometer at the previous timestamp is obtained, and the point cloud data collected by the single-line laser radar at the current timestamp is obtained, and the posture information collected by the foot odometer at the current timestamp is obtained. Based on the posture information collected at the previous timestamp and the position offset, the expected position of the current timestamp in the world coordinate system is calculated, and the point cloud data collected at the current timestamp and the expected position of the current timestamp are mapped to the world coordinate system. The nearest neighboring point of each point cloud in the point cloud data is searched in the world coordinate system, and the transformation matrix between each point cloud in the point cloud data and the nearest neighboring point is calculated. The expected position is multiplied by the transformation matrix to obtain the actual position of the current timestamp in the world coordinate system. This method is used to correct the expected position of the current timestamp in the world coordinate system, and the corrected expected position of the current timestamp is obtained, so that the actual position of the current timestamp can be obtained, and the real-time positioning of the robot is completed, and the accuracy of the robot positioning is improved.
[0118] The following is an embodiment of the device of the present application, which can be used to execute the embodiment of the method of the present application. For details not disclosed in the embodiment of the device of the present application, please refer to the embodiment of the method of the present application.
[0119] See also Figure 5 , which shows a schematic diagram of the structure of a robot positioning device provided by an exemplary embodiment of the present application. The robot positioning device can be implemented as all or part of a terminal through software, hardware or a combination of both. The device 1 includes a data acquisition module 11, a desired position calculation module 12, a transformation matrix calculation module 13, and an actual position calculation module 14.
[0120] A data acquisition module 11 is used to acquire the posture information collected by the foot odometer at the previous timestamp, and to acquire the point cloud data collected by the single-line laser radar at the current timestamp;
[0121] An expected position calculation module 12, used to calculate the expected position of the current timestamp in the world coordinate system based on the posture information collected at the previous timestamp;
[0122] A transformation matrix calculation module 13 is used to map the point cloud data collected at the current timestamp and the expected position of the current timestamp to the world coordinate system to obtain a transformation matrix;
[0123] The actual position calculation module 14 is used to calculate the actual position of the current timestamp in the world coordinate system based on the transformation matrix and the expected position.
[0124] Optional, such as Figure 6 As shown, the expected position calculation module 12 includes:
[0125] A current posture acquisition unit 121, used to acquire the posture information collected by the foot odometer at the current timestamp;
[0126] An offset calculation unit 122, configured to obtain a position offset of the foot odometer between the previous timestamp and the current timestamp based on the pose information collected at the previous timestamp and the pose information collected at the current timestamp;
[0127] The expected position calculation unit 123 is used to calculate the expected position of the current timestamp in the world coordinate system based on the posture information collected at the previous timestamp and the position offset.
[0128] Optionally, the offset calculation unit is specifically used to:
[0129]
[0130] Wherein, T1 is the posture information collected at the previous timestamp;
[0131] T2 is the posture information collected at the current timestamp;
[0132] The Tx is the position offset between the previous timestamp and the current timestamp.
[0133] Optional, such as Figure 7 As shown, the transformation matrix calculation module 13 includes:
[0134] A data mapping unit 131, configured to map the point cloud data collected at the current timestamp and the expected position of the current timestamp to the world coordinate system;
[0135] A nearest neighboring point search unit 132, used for searching the nearest neighboring point of each point cloud in the point cloud data in the world coordinate system;
[0136] The transformation matrix calculation unit 133 is used to calculate the transformation matrix between each point cloud in the point cloud data and the nearest neighboring point.
[0137] Optionally, the transformation matrix calculation unit is specifically used to calculate the transformation matrix between each point cloud in the point cloud data and the nearest adjacent point using a near K search algorithm.
[0138] Optionally, the actual position calculation module is specifically used to multiply the expected position by the transformation matrix to obtain the actual position of the current timestamp in the world coordinate system.
[0139] In an embodiment of the present application, the posture information collected by the foot odometer at the previous timestamp is obtained, and the point cloud data collected by the single-line laser radar at the current timestamp is obtained, and the posture information collected by the foot odometer at the current timestamp is obtained. Based on the posture information collected at the previous timestamp and the position offset, the expected position of the current timestamp in the world coordinate system is calculated, and the point cloud data collected at the current timestamp and the expected position of the current timestamp are mapped to the world coordinate system. The nearest neighboring point of each point cloud in the point cloud data is searched in the world coordinate system, and the transformation matrix between each point cloud in the point cloud data and the nearest neighboring point is calculated. The expected position is multiplied by the transformation matrix to obtain the actual position of the current timestamp in the world coordinate system. This method is used to correct the expected position of the current timestamp in the world coordinate system, and the corrected expected position of the current timestamp is obtained, so that the actual position of the current timestamp can be obtained, and the real-time positioning of the robot is completed, and the accuracy of the robot positioning is improved.
[0140] It should be noted that the robot positioning device provided in the above embodiment only uses the division of the above functional modules as an example when executing the robot positioning method. In actual applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the device is divided into different functional modules to complete all or part of the functions described above. In addition, the robot positioning device provided in the above embodiment and the robot positioning method embodiment belong to the same concept, and the implementation process thereof is detailed in the method embodiment, which will not be repeated here.
[0141] The serial numbers of the above-mentioned embodiments of the present application are for description only and do not represent the advantages or disadvantages of the embodiments.
[0142] The present application also provides a computer storage medium, which can store multiple instructions, and the instructions are suitable for being loaded and executed by a processor as described above. Figure 1-Figure 4 The method steps of the embodiment shown in the figure can be found in the specific implementation process. Figure 1-Figure 4 The specific description of the illustrated embodiment will not be repeated here.
[0143] The present application also provides a robot, which stores at least one instruction, and the at least one instruction is loaded and executed by the processor as described above. Figure 1-Figure 4 The method steps of the embodiment shown in the figure can be found in the specific implementation process. Figure 1-Figure 4 The specific description of the illustrated embodiment will not be repeated here.
[0144] See also Figure 8 , which is a schematic diagram of the structure of a robot provided in the embodiment of the present application. Figure 8 As shown, the robot 1000 may include: at least one processor 1001 , at least one network interface 1004 , a user interface 1003 , a memory 1005 , and at least one communication bus 1002 .
[0145] The communication bus 1002 is used to realize the connection and communication between these components.
[0146] The user interface 1003 may include a display screen (Display) and a camera (Camera), and the optional user interface 1003 may also include a standard wired interface and a wireless interface.
[0147] The network interface 1004 may optionally include a standard wired interface or a wireless interface (such as a WI-FI interface).
[0148] Among them, the processor 1001 may include one or more processing cores. The processor 1001 uses various interfaces and lines to connect various parts of the entire robot 1000, and executes various functions and processes data of the robot 1000 by running or executing instructions, programs, code sets or instruction sets stored in the memory 1005, and calling data stored in the memory 1005. Optionally, the processor 1001 can be implemented in at least one hardware form of digital signal processing (Digital Signal Processing, DSP), field programmable gate array (Field-Programmable Gate Array, FPGA), and programmable logic array (Programmable Logic Array, PLA). The processor 1001 can integrate one or a combination of a central processing unit (Central Processing Unit, CPU), a graphics processing unit (Graphics Processing Unit, GPU) and a modem. Among them, the CPU mainly processes the operating system, user interface and application programs; the GPU is responsible for rendering and drawing the content to be displayed on the display screen; and the modem is used to process wireless communication. It can be understood that the above-mentioned modem may not be integrated into the processor 1001, and it can be implemented separately through a chip.
[0149] Among them, the memory 1005 may include a random access memory (Random Access Memory, RAM) and may also include a read-only memory (Read-Only Memory). Optionally, the memory 1005 includes a non-transitory computer-readable storage medium. The memory 1005 can be used to store instructions, programs, codes, code sets or instruction sets. The memory 1005 may include a program storage area and a data storage area, wherein the program storage area may store instructions for implementing an operating system, instructions for at least one function (such as a touch function, a sound playback function, an image playback function, etc.), instructions for implementing the above-mentioned various method embodiments, etc.; the data storage area may store data involved in the above-mentioned various method embodiments, etc. The memory 1005 may optionally be at least one storage device located away from the aforementioned processor 1001. As Figure 8 As shown, the memory 1005 as a computer storage medium may include an operating system, a network communication module, a user interface module, and a robot positioning application.
[0150] exist Figure 8In the robot 1000 shown, the user interface 1003 is mainly used to provide an input interface for the user and obtain the data input by the user; and the processor 1001 can be used to call the robot positioning application stored in the memory 1005, and specifically perform the following operations:
[0151] Obtaining the posture information collected by the foot odometer at the previous timestamp, and obtaining the point cloud data collected by the single-line laser radar at the current timestamp;
[0152] Calculating the expected position of the current timestamp in the world coordinate system based on the pose information collected at the previous timestamp;
[0153] Mapping the point cloud data collected at the current timestamp and the expected position of the current timestamp to the world coordinate system to obtain a transformation matrix;
[0154] The actual position of the current timestamp in the world coordinate system is calculated based on the transformation matrix and the expected position.
[0155] In one embodiment, when the processor 1001 calculates the expected position of the current timestamp in the world coordinate system based on the posture information collected at the previous timestamp, the processor 1001 specifically performs the following operations:
[0156] Obtaining the position and posture information collected by the foot odometer at the current timestamp;
[0157] Based on the posture information collected at the previous timestamp and the posture information collected at the current timestamp, obtaining a position offset of the foot odometer between the previous timestamp and the current timestamp;
[0158] Based on the pose information collected at the previous timestamp and the position offset, the expected position of the current timestamp in the world coordinate system is calculated.
[0159] In one embodiment, when the processor 1001 obtains the position offset of the foot odometer between the previous timestamp and the current timestamp based on the posture information collected at the previous timestamp and the posture information collected at the current timestamp, the processor 1001 specifically performs the following operations:
[0160] Position offset
[0161] Wherein, T1 is the posture information collected at the previous timestamp;
[0162] T2 is the posture information collected at the current timestamp;
[0163] The Tx is the position offset between the previous timestamp and the current timestamp.
[0164] In one embodiment, the processor 1001 maps the point cloud data collected at the current timestamp and the expected position of the current timestamp to the world coordinate system to obtain a transformation matrix by specifically performing the following operations:
[0165] Mapping the point cloud data collected at the current timestamp and the expected position of the current timestamp to the world coordinate system;
[0166] Searching for the nearest neighboring point of each point cloud in the point cloud data in the world coordinate system;
[0167] A transformation matrix between each point cloud in the point cloud data and the nearest neighboring point is calculated.
[0168] In one embodiment, the processor 1001 performs the following operations when calculating the transformation matrix between each point cloud in the point cloud data and the nearest neighboring point:
[0169] A near K search algorithm is used to calculate the transformation matrix between each point cloud in the point cloud data and the nearest neighboring point.
[0170] In one embodiment, when the processor 1001 calculates the actual position of the current timestamp in the world coordinate system based on the transformation matrix and the expected position, the processor 1001 specifically performs the following operations:
[0171] The expected position is multiplied by the transformation matrix to obtain the actual position of the current timestamp in the world coordinate system.
[0172] In an embodiment of the present application, the posture information collected by the foot odometer at the previous timestamp is obtained, and the point cloud data collected by the single-line laser radar at the current timestamp is obtained, and the posture information collected by the foot odometer at the current timestamp is obtained. Based on the posture information collected at the previous timestamp and the position offset, the expected position of the current timestamp in the world coordinate system is calculated, and the point cloud data collected at the current timestamp and the expected position of the current timestamp are mapped to the world coordinate system. The nearest neighboring point of each point cloud in the point cloud data is searched in the world coordinate system, and the transformation matrix between each point cloud in the point cloud data and the nearest neighboring point is calculated. The expected position is multiplied by the transformation matrix to obtain the actual position of the current timestamp in the world coordinate system. This method is used to correct the expected position of the current timestamp in the world coordinate system, and the corrected expected position of the current timestamp is obtained, so that the actual position of the current timestamp can be obtained, and the real-time positioning of the robot is completed, and the accuracy of the robot positioning is improved.
[0173] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be implemented by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium, and when the program is executed, it can include the processes of the embodiments of the above-mentioned methods. The storage medium can be a disk, an optical disk, a read-only storage memory, or a random access memory, etc.
[0174] The above disclosure is only the preferred embodiment of the present application, which certainly cannot be used to limit the scope of rights of the present application. Therefore, equivalent changes made according to the claims of the present application are still within the scope covered by the present application.
Claims
1. A robot positioning method, characterized in that: Applied to a robot, the robot includes a foot odometer and a single-line laser radar, and the method includes: Obtaining the posture information collected by the foot odometer at the previous timestamp, and obtaining the point cloud data collected by the single-line laser radar at the current timestamp; Obtaining the position and posture information collected by the foot odometer at the current timestamp; Obtaining a position offset of the foot odometer between the previous timestamp and the current timestamp based on the posture information collected at the previous timestamp and the posture information collected at the current timestamp by the following formula; Wherein, T1 is the posture information collected at the previous timestamp, T2 is the posture information collected at the current timestamp, and Tx is the position offset between the previous timestamp and the current timestamp; Adding the posture information collected at the previous timestamp to the position offset to obtain the expected position of the current timestamp; Mapping the point cloud data collected at the current timestamp and the expected position of the current timestamp to a world coordinate system; Searching for the nearest neighboring point of each point cloud in the point cloud data in the world coordinate system; Calculating a transformation matrix between each point cloud in the point cloud data and the nearest adjacent point; The actual position of the current timestamp in the world coordinate system is calculated based on the transformation matrix and the expected position.
2. The method according to claim 1, characterized in that The calculating of the transformation matrix between each point cloud in the point cloud data and the nearest neighboring point includes: A near K search algorithm is used to calculate the transformation matrix between each point cloud in the point cloud data and the nearest neighboring point.
3. The method according to claim 1, characterized in that The calculating the actual position of the current timestamp in the world coordinate system based on the transformation matrix and the expected position includes: The expected position is multiplied by the transformation matrix to obtain the actual position of the current timestamp in the world coordinate system.
4. A robot positioning device, characterized in that: Applied to a robot, the robot includes a foot odometer and a single-line laser radar, and the device includes: A data acquisition module, used to acquire the posture information collected by the foot odometer at the previous timestamp, and to acquire the point cloud data collected by the single-line laser radar at the current timestamp; An expected position calculation module is used to obtain the posture information collected by the foot odometer at the current timestamp; based on the posture information collected at the previous timestamp and the posture information collected at the current timestamp, obtain the position offset of the foot odometer between the previous timestamp and the current timestamp through the following formula; Wherein, T1 is the posture information collected at the previous timestamp, T2 is the posture information collected at the current timestamp, and Tx is the position offset between the previous timestamp and the current timestamp; Adding the posture information collected at the previous timestamp to the position offset to obtain the expected position of the current timestamp; A transformation matrix calculation module is used to map the point cloud data collected at the current timestamp and the expected position of the current timestamp to a world coordinate system; search for the nearest neighboring point of each point cloud in the point cloud data in the world coordinate system; and calculate the transformation matrix between each point cloud in the point cloud data and the nearest neighboring point; The actual position calculation module is used to calculate the actual position of the current timestamp in the world coordinate system based on the transformation matrix and the expected position.
5. A computer storage medium, characterized in that: The computer storage medium stores a plurality of instructions, which are suitable for being loaded by a processor and executing the method steps according to any one of claims 1 to 3.
6. A robot, characterized in that: include: A processor and a memory; wherein the memory stores a computer program, and the computer program is suitable for being loaded by the processor and executing the method steps as claimed in any one of claims 1 to 3.
