A positioning method, a positioning device, a robot, and a storage medium

Abstract

The application discloses a positioning method, a positioning device, a robot and a computer readable storage medium. The method comprises the following steps: after collecting original point cloud data at a current time through a laser radar of the robot, preprocessing the original point cloud data to obtain target point cloud data; determining positioning optimization data at the current time, wherein the positioning optimization data is used for optimizing the positioning process of the robot; and obtaining a positioning result of the robot based on the target point cloud data, the positioning optimization data and a preset environment point cloud map. Through the application, the positioning accuracy and the positioning precision of the robot can be improved.

Description

Technical Field

[0001] This application belongs to the field of navigation technology, and in particular relates to a positioning method, positioning device, robot, and computer-readable storage medium. Background Technology

[0002] Compared to traditional mobile robots, biomimetic quadruped robots exhibit a more biologically similar locomotion pattern and greater freedom of movement. Furthermore, the amplitude of motion changes more dramatically and rapidly, and the frequency of these changes is higher. These factors present certain challenges to the localization of biomimetic quadruped robots. Summary of the Invention

[0003] This application provides a positioning method, positioning device, robot, and computer-readable storage medium, which can improve the positioning accuracy and precision of the robot.

[0004] Firstly, this application provides a positioning method, including: After acquiring the raw point cloud data at the current moment through the robot's lidar, the raw point cloud data is preprocessed to obtain the target point cloud data; Determine the positioning optimization data at the current moment, and the positioning optimization data is used to optimize the robot's positioning process; Based on the order of resolution from low to high, one layer of environmental point cloud map is determined as the current environmental point cloud map in the preset environmental point cloud map. The preset environmental point cloud map is an environmental point cloud map with two or more layers, and the resolution of the uppermost environmental point cloud map is lower, while the resolution of the lowermost environmental point cloud map is higher. If the current environment point cloud map is the top layer, then the preset default value is used as the initial value for point cloud registration. Point cloud registration is performed based on the target point cloud data and the current environment point cloud map to obtain the original positioning result of the current environment point cloud map. If the current environmental point cloud map is not the top layer, the optimized positioning result of the previous layer environmental point cloud map is used as the initial value for point cloud registration. Point cloud registration is performed based on the target point cloud data and the current environmental point cloud map to obtain the original positioning result of the current environmental point cloud map. The optimized positioning data and the original positioning results of the current environment point cloud map are fused based on a preset lossless Kalman filter to obtain the optimized positioning result of the current environment point cloud map. If the current environment point cloud map is the lowest layer, then the optimized positioning result of the current environment point cloud map is determined as the positioning result of the robot.

[0005] Secondly, this application provides a positioning device, comprising: The extraction module is used to preprocess the raw point cloud data collected by the robot's lidar at the current moment to obtain the target point cloud data. The determination module is used to determine the positioning optimization data at the current moment, and the positioning optimization data is used to optimize the positioning process of the robot. The positioning module is used to obtain the positioning result of the robot based on the target point cloud data, the positioning optimization data and the preset environmental point cloud map. The positioning module includes: The current map determination sub-unit is used to determine one layer of environmental point cloud map as the current environmental point cloud map in the two or more layers of environmental point cloud maps in order of resolution from low to high. The preset environmental point cloud map is an environmental point cloud map with two or more layers, and the resolution of the uppermost environmental point cloud map is lower, while the resolution of the lowermost environmental point cloud map is higher. The current map point cloud registration subunit is used to perform point cloud registration based on the target point cloud data and the current environment point cloud map, if the current environment point cloud map is the top layer, by using a preset default value as the initial value for point cloud registration, and obtaining the original positioning result of the current environment point cloud map; if the current environment point cloud map is not the top layer, by using the optimized positioning result of the previous layer environment point cloud map as the initial value for point cloud registration, and performing point cloud registration based on the target point cloud data and the current environment point cloud map, and obtaining the original positioning result of the current environment point cloud map. The current map positioning optimization subunit is used to fuse the positioning optimization data and the original positioning results of the current environment point cloud map based on a preset lossless Kalman filter to obtain the optimized positioning result of the current environment point cloud map. The positioning result determination subunit is used to determine the optimized positioning result of the current environment point cloud map as the positioning result of the robot if the current environment point cloud map is the lowest level.

[0006] Thirdly, this application provides a robot, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the method described in the first aspect.

[0007] Fourthly, this application provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the method described in the first aspect above.

[0008] Fifthly, this application provides a computer program product comprising a computer program that, when executed by one or more processors, implements the steps of the method described in the first aspect.

[0009] The beneficial effects of this application compared to existing technologies are as follows: After the robot collects the raw point cloud data at the current moment using lidar, it preprocesses the raw point cloud data to obtain target point cloud data, and then determines the positioning optimization data at the current moment. This positioning optimization data is used to optimize the robot's positioning process. Finally, based on the target point cloud data, the positioning optimization data, and a preset environmental point cloud map, the robot's positioning result is obtained. A new parameter, positioning optimization data, is introduced into the robot's positioning process. This optimization data optimizes the robot's positioning process, ensuring the accuracy and precision of the final positioning result. It is understood that the beneficial effects of the second to fifth aspects mentioned above can be found in the relevant descriptions in the first aspect above, and will not be repeated here. Attached Figure Description

[0010] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0011] Figure 1 This is a schematic diagram illustrating the implementation process of the positioning method provided in the embodiments of this application; Figure 2 This is a first workflow example diagram of the positioning method provided in the embodiments of this application; Figure 3 This is a second workflow example diagram of the positioning method provided in the embodiments of this application; Figure 4 This is a third example flowchart of the positioning method provided in the embodiments of this application; Figure 5 This is an example diagram of a three-layer environmental point cloud map provided in an embodiment of this application; Figure 6 This is a fourth workflow example diagram of the positioning method provided in the embodiments of this application; Figure 7 This is a fifth workflow example diagram of the positioning method provided in the embodiments of this application; Figure 8 This is a sixth flowchart illustrating the positioning method provided in this application. Figure 9This is the seventh workflow example diagram of the positioning method provided in the embodiments of this application; Figure 10 This is a structural block diagram of the positioning device provided in the embodiments of this application; Figure 11 This is a schematic diagram of the robot provided in the embodiments of this application. Detailed Implementation

[0012] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this application with unnecessary detail.

[0013] To illustrate the technical solution proposed in this application, specific embodiments are described below.

[0014] The following describes a positioning method provided by an embodiment of this application. This positioning method is applied to a robot equipped with a lidar system. Please refer to... Figure 1 The positioning method includes: Step 101: After acquiring the raw point cloud data at the current moment through the robot's lidar, preprocess the raw point cloud data to obtain the target point cloud data.

[0015] In this embodiment, during operation, the robot can acquire raw point cloud data at the current moment using the LiDAR's operating clock. After obtaining the raw point cloud data, it can be preprocessed. For ease of explanation, the result of the preprocessing is referred to as the target point cloud data.

[0016] As an example only, the preprocessing may include voxel filtering to downsample the original point cloud data; the preprocessing may also include feature extraction to extract ground feature point cloud data and ground feature point cloud data that can constitute geometric features from the original point cloud data; wherein, the ground feature point cloud data may further include ground line feature point cloud data and ground surface feature point cloud data.

[0017] Step 102: Determine the positioning optimization data for the current moment.

[0018] In this embodiment, the robot can perform positioning or assisted positioning through preset components on its body to obtain positioning optimization data at the current moment. It can be understood that this positioning optimization data is used to optimize the robot's current positioning process; that is, the positioning optimization data plays a role in optimizing the positioning result during the robot's current positioning process. Specifically, the preset component can be the LiDAR itself, or other components of the robot besides the LiDAR, and is not limited here.

[0019] Step 103: Based on the target point cloud data, the positioning optimization data, and the preset environmental point cloud map, the positioning result of the robot is obtained.

[0020] In this embodiment, the environmental point cloud map can be pre-obtained using a laser-based simultaneous localization and mapping (SLAM) method. The environmental point cloud map is pre-divided into multiple grids of the same size, and is typically represented as a PCD format file. It is understood that the subsequent localization results can be represented based on the map coordinate system of this environmental point cloud map.

[0021] Based on the target point cloud data, the robot can perform point cloud registration with the environmental point cloud map to complete its localization operation. Specifically, point cloud registration can be achieved based on the Normal Distributions Transform (NDT) algorithm. The process is as follows: the environmental point cloud map is divided into K grids. For each target point cloud data, the distance between the target point cloud data and each grid is calculated. Point cloud matching is performed based on the distance between each target point cloud data and each grid, and optimization is performed to minimize the distance between each target point cloud data and its corresponding grid, thereby achieving the localization of the robot. During this optimization process, an initial value P0 for rotation and translation (i.e., pose) is used. It can be understood that the default value of this initial value P0 is a 0 vector, meaning the initial value for rotation is a 0 vector and the initial value for translation is a 0 vector.

[0022] The robot can optimize the localization process based on point cloud registration using localization optimization data. In this application, two different types of localization optimization data are proposed: one type optimizes the localization data obtained after point cloud registration to obtain the final localization result; the other type optimizes the localization process itself to obtain the final localization result. The optimization operations corresponding to these two types of localization optimization data can also be understood as: optimization within point cloud registration and optimization outside of point cloud registration.

[0023] In some embodiments, step 102 can be implemented through the following process: A1. Based on the output of the robot's inertial measurement unit, determine the robot's first rotation data and first translation data from the previous moment to the current moment.

[0024] A2. Based on the first rotation data and the first translation data mentioned above, the pose of the robot at the current moment is predicted to obtain the first predicted pose.

[0025] A3. The first predicted pose is determined as the above-mentioned positioning optimization data.

[0026] The robot can integrate an Inertial Measurement Unit (IMU). Since the IMU contains a gyroscope and accelerometer, rotation and translation data over a period of time can be calculated from its output. It's important to note that the IMU's operating clock differs from that of the lidar, specifically in that the IMU's clock has higher precision. That is, between two adjacent times recorded by the lidar, there may be multiple times recorded by the IMU. In this embodiment, both the previous time and the current time are times recorded by the lidar's operating clock. Therefore, it is necessary to select the output from the IMU that is closest to the lidar's previous time and the output that is closest to the lidar's current time for calculating rotation and translation data. For ease of distinction, this rotation data is denoted as the first rotation data, and this translation data as the first translation data.

[0027] The robot then predicts its pose at the current moment based on the first translation data and the first rotation data, thereby obtaining the first predicted pose, which can be used as the localization optimization data at the current moment.

[0028] In some embodiments, step 102 can also be implemented through the following process: B1. Based on the output of the robot's inertial measurement unit, determine the robot's second rotation data from the previous moment to the current moment.

[0029] B2. Based on the output of the ultra-wideband tag of the robot, determine the second translation data of the robot from the previous time to the current time.

[0030] B3. Based on the second rotation data and the second translation data mentioned above, the pose of the robot at the current moment is predicted to obtain the second predicted pose.

[0031] B4. The second predicted pose is determined as the above-mentioned positioning optimization data.

[0032] The difference between steps B1-B4 and steps A1-A3 is as follows: In steps A1-A3, rotation and translation data are determined directly based on the IMU output; while in steps B1-B4, rotation data is determined only based on the IMU output, and translation data is determined by the output of the robot's Ultra Wide Band (UWB) tag. It can be understood that, under normal operating conditions of the UWB base station and UWB tag, the translation data obtained based on the robot's UWB tag output (i.e., the second translation data) has higher accuracy and is less prone to disconnection. Furthermore, this UWB tag is synchronized with the LiDAR by default.

[0033] In some embodiments, in application scenarios where positioning optimization data is obtained based on IMU, step 103 can be implemented through the following process: C1. Based on the above target point cloud data and the above environmental point cloud map, point cloud registration is performed to obtain the first original positioning result.

[0034] C2. Based on the preset error equation, the above-mentioned positioning optimization data and the above-mentioned first original positioning result are nonlinearly optimized to obtain the above-mentioned positioning result.

[0035] It is understood that the initial value P0 used in the point cloud registration operation in step C1 is the default value. For ease of distinction, the original positioning result obtained after the point cloud registration in step C1 is denoted as the first original positioning result. In this embodiment, a preset error equation is introduced to perform nonlinear optimization on the positioning optimization data (i.e., the positioning optimization data obtained in steps A1-A3) and the first original positioning result to obtain the final positioning result.

[0036] As an example only, the optimization term in the nonlinear optimization process is: [xyzrpyv x v y v z b ax b ay b az b gx b gy b gz ] Where x, y, z, r, p, and y represent the robot's pose. Specifically, x is the robot's coordinate on the X-axis of the map coordinate system, y is the robot's coordinate on the Y-axis of the map coordinate system, z is the robot's coordinate on the Z-axis of the map coordinate system, r is the robot's roll angle, p is the robot's pitch angle, and y is the robot's yaw angle.

[0037] v x v y and v zThis indicates the robot's speed. Specifically, v x v is the robot's velocity on the X-axis. y v is the robot's velocity on the Y-axis. z This represents the robot's speed along the Z-axis.

[0038] b ax b ay and b az This indicates the bias of the IMU's accelerometer. Specifically, b ax b represents the accelerometer's deviation on the X-axis. ay b represents the accelerometer's deviation on the Y-axis. az This represents the accelerometer's deviation on the Z-axis.

[0039] b gx b gy and b gz This indicates the deviation of the IMU's gyroscope. Specifically, b gx b represents the gyroscope's deviation on the X-axis. gy b represents the gyroscope's deviation on the Y-axis. gz This represents the deviation of the gyroscope on the Z-axis.

[0040] The error equation is as follows:

[0041] Where F is the error value, E L This is the error term for point cloud registration based on lidar (i.e., the first original positioning result). E I Σ represents the error term for IMU-based prediction (i.e., the first predicted pose). L Let Σ be the covariance of the lidar. I Let be the covariance of the IMU, a constant. This is to facilitate differentiation. By using the Gauss-Newton method to optimize the error equation, we can obtain the optimization term that minimizes the error value F, thus yielding the positioning result.

[0042] For easier understanding, please refer to Figure 2 , Figure 2 A workflow example of the positioning method based on steps A1-A3 and C1-C2 is given. As can be seen from the optimization terms, the input to this nonlinear optimization, in addition to the positioning optimization data and the first original positioning result, also includes the IMU parameters, i.e., the IMU bias data, which is not included in... Figure 2As shown in the diagram. When this positioning method is applied to a robot using a solid-state multi-line LiDAR, considering that the field of view of the solid-state multi-line LiDAR is 140° horizontally and 60° vertically, it is limited by the field of view of the solid-state multi-line LiDAR, and therefore, large-scale feature extraction is usually not performed on the original point cloud data. In this application scenario, the preprocessing performed in step 101 may include: voxel filtering-based downsampling.

[0043] In some embodiments, in application scenarios where location optimization data is obtained based on IMU and UWB, or in application scenarios where location optimization data is obtained based on IMU, step 103 can also be implemented through the following process: D1. Based on the above target point cloud data and the above environmental point cloud map, point cloud registration is performed to obtain the second original positioning result.

[0044] D2. Based on the preset lossless Kalman filter, the above-mentioned positioning optimization data and the above-mentioned second original positioning result are fused to obtain the above-mentioned positioning result.

[0045] It is understood that the initial value P0 used in the point cloud registration operation in step D1 is the default value. For ease of distinction, the original positioning result obtained after the point cloud registration in step D1 is recorded as the second original positioning result. In this embodiment, the positioning optimization data (i.e., the positioning optimization data obtained in steps A1-A3, or the positioning optimization data obtained in steps B1-B4) and the second original positioning result can be fused based on the Unscented Kalman Filter (UKF) to obtain the final positioning result.

[0046] For easier understanding, please refer to Figure 3 , Figure 3 A workflow example based on steps A1-A3 and D1-D2 is provided. When this localization method is applied to a robot using a solid-state multi-line LiDAR, considering that the field of view of the solid-state multi-line LiDAR is 140° horizontally and 60° vertically, the original point cloud data is typically not extensively feature-extracted due to the limited field of view of the solid-state multi-line LiDAR. In this application scenario, the preprocessing performed in step 101 may include voxel filtering-based downsampling.

[0047] For easier understanding, please refer to Figure 4 , Figure 4A workflow example based on steps B1-B4 and D1-D2 is provided. When this localization method is applied to a robot using a solid-state multi-line LiDAR, considering that the field of view of the solid-state multi-line LiDAR is 140° horizontally and 60° vertically, the field of view is limited, and large-scale feature extraction of the original point cloud data is usually not performed. In this application scenario, the preprocessing in step 101 may include voxel filtering-based downsampling.

[0048] In some embodiments, to improve positioning speed, the robot can also construct multi-layered environmental point cloud maps; that is, the preset environmental point cloud map can be two or more layers deep, with higher layers having lower resolution and lower layers having higher resolution. In fact, the environmental point cloud data in each layer is completely identical; the only difference is the size of the grid. It can be understood that the larger the grid size in the environmental point cloud map, the fewer the number of grids, resulting in lower resolution and lower accuracy; conversely, the smaller the grid size, the more the number of grids, resulting in higher resolution and higher accuracy. In other words, when the original environmental point cloud map (i.e., the ungrided environmental point cloud map) is obtained beforehand using the SLAM method, it can be divided into layers with given grid sizes from coarse to fine, thus obtaining two or more layers of environmental point cloud maps with different resolutions.

[0049] like Figure 5 As shown, Figure 5 An example of a three-layer environmental point cloud map is given. The bottom layer of the environmental point cloud map can have the smallest grid size (e.g., 0.5m × 0.5m × 0.5m); for each layer above the bottom layer, the corresponding grid size increases to twice the size of the next layer.

[0050] When the preset environmental point cloud map has two or more layers, steps D1 and D2 above can be optimized. Step D1 can be implemented through the following process: D11. Based on the resolution from low to high, determine one environmental point cloud map from the above two or more layers of environmental point cloud maps as the current environmental point cloud map.

[0051] D12. If the current environment point cloud map is the top layer, then the preset default value is used as the initial value for point cloud registration. Point cloud registration is performed based on the target point cloud data and the current environment point cloud map to obtain the second original positioning result of the current environment point cloud map.

[0052] D13. If the current environment point cloud map is not the top layer, the optimized positioning result of the previous layer environment point cloud map is used as the initial value for point cloud registration. Point cloud registration is performed based on the target point cloud data and the current environment point cloud map to obtain the second original positioning result of the current environment point cloud map.

[0053] Step D2 can be achieved through the following process: D21. Based on the preset lossless Kalman filter, the above-mentioned positioning optimization data and the second original positioning result of the above-mentioned current environment point cloud map are fused to obtain the above-mentioned optimized positioning result of the current environment point cloud map.

[0054] D22. If the current environment point cloud map is the bottom layer, then the optimized positioning result of the current environment point cloud map is determined as the above positioning result.

[0055] For easier understanding, please refer to Figure 6 This paper presents a workflow example of the localization method based on steps D11-D13 and D21-D22. When this localization method is applied to a robot using a solid-state multi-line LiDAR, considering that the field of view of the solid-state multi-line LiDAR is 140° horizontally and 60° vertically, the field of view is limited, and large-scale feature extraction of the original point cloud data is usually not performed. In this application scenario, the preprocessing in step 101 may include voxel filtering-based downsampling.

[0056] Based on this Figure 6 Taking a 3-layer environmental point cloud map as an example: Starting from the highest layer (i.e., the layer with the lowest resolution), point cloud registration is performed between the environmental point cloud map and the target point cloud data at this highest layer to obtain the second original localization result for this highest layer. It is important to note that the initial values ​​used in this point cloud registration process are default values. Based on UKF, the localization optimization data and the second original localization result from this highest layer are fused to obtain the optimized localization result for this highest layer. This optimized localization result from the highest layer will be passed to the next layer (i.e., the second layer) as the initial value used for point cloud registration based on the second layer.

[0057] Subsequently, point cloud registration is performed between the environmental point cloud map and the target point cloud data of this second layer to obtain the second original localization result of the second layer. It is important to note that the initial value used in this point cloud registration process is the optimized localization result of the highest layer. Based on UKF, the optimized localization data and the second original localization result of the second layer are fused to obtain the optimized localization result of the second layer. This optimized localization result of the second layer will be passed to the next layer (i.e., the third layer) as the initial value used when performing point cloud registration based on the third layer.

[0058] Finally, point cloud registration is performed between the environmental point cloud map and the target point cloud data of the third layer to obtain the second original localization result of the third layer. It is important to note that the initial value used in this point cloud registration process is the optimized localization result of the second layer. Based on UKF, the optimized localization data and the second original localization result of the third layer are fused to obtain the optimized localization result of the third layer. Since the third layer is the lowest layer, its optimized localization result can be output as the robot's final localization result. Using a multi-layered environmental point cloud map, from coarse to fine, can improve the computation speed during the localization process, thus increasing localization efficiency.

[0059] All of the above positioning methods can be understood as optimizations based on point cloud registration.

[0060] In some embodiments, step 102 can also be implemented through the following process: E1. Obtain the positioning results of the robot based on the aforementioned LiDAR at a historical moment.

[0061] E2. Based on the positioning results at the above historical moments, determine the optimized positioning data for the current moment.

[0062] This historical moment specifically refers to the N moments preceding the current moment. For ease of understanding, let's denote the current moment as T0. The N moments preceding the current moment are: T-1 (when N is 1); T-1 and T-2 (when N is 2); T-1, T-2, and T-3 (when N is 3), and so on. It's worth noting that these moments also refer to the time displayed on the lidar's operating clock.

[0063] In one application scenario, N is 1; then step E1 can be expressed as: determining the positioning results of the previous N time steps as the positioning optimization data for the current time step. Step E2 can be expressed as: using the above positioning optimization data as the initial value for point cloud registration, performing point cloud registration based on the above target point cloud data and the above environmental point cloud map to obtain the above positioning result.

[0064] That is, when the current time is T0, the localization result at time T-1 is determined as the localization optimization data at time T0. When performing point cloud registration based on the target point cloud data and the environmental point cloud map at the current time, this localization optimization data (i.e., the localization result at time T-1) will be used as the initial value to optimize the point cloud registration process and thus obtain the robot's localization result.

[0065] It is understandable that when the current time is time 0, there is no previous time. Therefore, the positioning optimization data at time 0 is empty. Accordingly, when performing point cloud registration based on the target point cloud data and the environmental point cloud map at time 0, the initial value used is the default value.

[0066] For easier understanding, please refer to Figure 7 Figure 7 provides a workflow example of the localization method based on steps E1 and E2 when N is 1. When this localization method is applied to a robot using a rotating multi-line LiDAR, its large field of view allows for extensive feature extraction from the raw point cloud data. In this application scenario, the preprocessing performed in step 101 may include feature extraction.

[0067] In another application scenario, N is any other positive integer greater than 1; then step E1 can be expressed as: performing a quadratic polynomial fitting on the pose vector based on the localization results of the previous N time steps, and predicting the robot's pose at the current time step based on the fitting result to obtain a third prediction result, which is then determined as the localization optimization data at the current time step. Step E2 can be expressed as: using the localization optimization data as the initial value for point cloud registration, performing point cloud registration based on the target point cloud data and the environmental point cloud map to obtain the localization result.

[0068] For the first N time steps, pose vectors can be formed based on the poses at these N time steps, namely vectors x, y, z, r, p, and y. Each vector has a size of N×1. These six vectors are fitted using quadratic polynomials, specifically: the vector values ​​are used as the ordinate data, and the abscissa data are [1, ..., N]. This yields six sets of polynomial parameters. The parameters of these six sets of quadratic polynomials are used to predict the element value corresponding to each vector when the abscissa is N+1, serving as the localization optimization data for time step T. When performing point cloud registration based on the target point cloud data and the environmental point cloud map at the current time step, this localization optimization data (i.e., the predicted element value corresponding to each vector when the abscissa is N+1) will be used as the initial value to optimize the point cloud registration process, thereby obtaining the robot's localization result.

[0069] It is understandable that when the current time is from time 0 to time N, there are no previous N time periods. Therefore, the localization optimization data from time 0 to time N is empty. Accordingly, when performing point cloud registration based on the target point cloud data from time 0 to time N and the environmental point cloud map, the initial value used is the default value.

[0070] For easier understanding, please refer to Figure 8Figure 8 provides a workflow example of the localization method based on steps E1 and E2 when N is 5. When this localization method is applied to a robot using a solid-state multi-line LiDAR, considering that the field of view of this solid-state multi-line LiDAR is 140° horizontally and 60° vertically, it is limited by the field of view of the solid-state multi-line LiDAR, and therefore, large-scale feature extraction of the original point cloud data is usually not performed. In this application scenario, the preprocessing performed in step 101 may include: voxel filtering-based downsampling.

[0071] In some embodiments, the robot may be moved and then restarted after being powered off. After restarting, since the IMU output is unavailable, step 102 can also be performed through the following process: F1. Based on the output of the ultra-wideband tag of the robot, determine the initial value of the predicted translation of the robot.

[0072] F2. Determine the initial value of the predicted rotation of the robot, which includes at least two specified rotation angles.

[0073] F3. The initial values ​​of the predicted translation and the initial values ​​of the predicted rotation are determined as the positioning optimization data.

[0074] In other words, the robot can perform relocalization based on its UWB tag. As described earlier, when both the UWB base station and the UWB tag are functioning normally, a relatively accurate robot location can be obtained through UWB technology. Therefore, the initial value of the robot's predicted translation can be determined based on the output of the robot's UWB tag. For the robot's rotation, several possible rotation angles can be preset as the initial value of the predicted rotation, such as 0°, 30°, 60°, and 120°. These initial values ​​of the predicted translation and rotation can then be determined as localization optimization data.

[0075] Accordingly, step 103 can be achieved through the following process: G1. Combine the initial values ​​of the predicted translation with each specified rotation angle in the initial values ​​of the predicted rotation to obtain at least one set of positioning optimization data to be determined.

[0076] G2. Using the optimization data of each group of undetermined positioning as the initial value for point cloud registration, point cloud registration is performed based on the above target point cloud data and the above environmental point cloud map to obtain at least one undetermined positioning result.

[0077] G3. Among the above at least one positioning result to be determined, the optimal positioning result to be determined is determined as the positioning result of the robot.

[0078] It is understandable that only one initial value for translation and one initial value for rotation are needed in a single point cloud registration process. Based on this, the obtained initial values ​​for predicted translation can be combined with various specified rotation angles to obtain at least one set of localization optimization data to be determined. As an example, assume the initial value for predicted translation is t1, and the initial values ​​for predicted rotation include r1, r2, r3…r… M For these M rotation angles, t1 and r1 form one set of positioning optimization data to be determined, t1 and r2 form another set, and so on, resulting in M ​​sets of positioning optimization data to be determined. The robot can use these M sets of positioning optimization data as initial values ​​to perform M point cloud registration operations, thus obtaining M positioning results to be determined. The optimal positioning result among these M results is the robot's final positioning result at the current moment. Specifically, the quality of the positioning result is judged by the magnitude of the LiDAR scanning matching error term; the smaller the error value, the better.

[0079] For easier understanding, please refer to Figure 9 , Figure 9 A workflow example based on steps F1-F3 and G1-G3 is provided. When this localization method is applied to a robot using a solid-state multi-line LiDAR, considering that the field of view of the solid-state multi-line LiDAR is 140° horizontally and 60° vertically, the field of view is limited, and large-scale feature extraction of the original point cloud data is usually not performed. In this application scenario, the preprocessing in step 101 may include voxel filtering-based downsampling.

[0080] All of the above positioning methods can be understood as being achieved through optimization within point cloud registration.

[0081] In some embodiments, point cloud registration involves calculating the distance between the target point cloud data and the grid in the environmental point cloud map. Specifically, the distance between a target point cloud and a grid refers to the distance between the target point cloud and the Gaussian distribution calculated based on that grid. In practical robot applications, all target point cloud data falling into the grid in the environmental point cloud map can first be considered as a point group. Then, the mean and covariance of this point group are calculated to construct a Gaussian distribution, which is the Gaussian distribution calculated based on the grid. Finally, the distance from the target point cloud to this Gaussian distribution is calculated and taken as the distance between the target point cloud and the grid.

[0082] In some embodiments, for a robot employing a rotating multi-line LiDAR, the specific steps for feature extraction from the raw point cloud data are as follows: First, since each laser beam of the rotating multi-line LiDAR has its own inherent code, a possible ground point set G' can be roughly selected based on this code. Next, the Euclidean distance between adjacent beam points in the ground point set G' is calculated and compared with a preset distance threshold t. If the Euclidean distance is less than the distance threshold t, the adjacent beam points corresponding to the Euclidean distance are merged into ground point G (i.e., ground feature point cloud data); otherwise, they are merged into ground point U. Then, ground point U is divided into n sectors. For each sector, the distance difference between each laser line point and its five surrounding points is calculated and divided by its own modulus to obtain a judgment value c. Based on experience, if c exceeds the empirical threshold c0, the point is added to the line feature point E (i.e., ground feature line point cloud data); if c does not exceed the empirical threshold c0, the point is added to the surface feature point S (i.e., ground feature surface point cloud data). Finally, the line feature points E, surface feature points S, and ground points G of all sectors are combined to form feature point cloud data, which is the result of feature extraction.

[0083] As can be seen from the above, through the solution of this application, after the robot collects the raw point cloud data at the current moment using LiDAR, it preprocesses the raw point cloud data to obtain target point cloud data, and then determines the positioning optimization data at the current moment. The positioning optimization data is used to optimize the robot's positioning process. Finally, based on the target point cloud data, the positioning optimization data, and the preset environmental point cloud map, the robot's positioning result is obtained. A new parameter, positioning optimization data, is introduced into the robot's positioning process. This optimization data optimizes the robot's positioning process to ensure the accuracy and precision of the final positioning result.

[0084] Corresponding to the positioning method proposed above, this application provides a positioning device, which is applied to a robot equipped with a lidar system. Please refer to... Figure 10 The positioning device 1000 in this embodiment includes: The extraction module 1001 is used to preprocess the raw point cloud data collected by the robot's lidar at the current moment to obtain the target point cloud data. The determination module 1002 is used to determine the positioning optimization data at the current moment, and the positioning optimization data is used to optimize the positioning process of the robot. The positioning module 1003 is used to obtain the positioning result of the robot based on the target point cloud data, the positioning optimization data and the preset environmental point cloud map.

[0085] Optionally, the determining module 1002 mentioned above includes: The first rotation data determination unit is used to determine the first rotation data of the robot from the previous moment to the current moment based on the output of the robot's inertial measurement unit. The first translation data determination unit is used to determine the first translation data of the robot from the previous moment to the current moment based on the output of the robot's inertial measurement unit. The first pose prediction unit is used to predict the pose of the robot at the current moment based on the first rotation data and the first translation data, and obtain the first predicted pose. The first positioning optimization data determination unit is used to determine the first predicted pose as the positioning optimization data.

[0086] Optionally, the determining module 1002 mentioned above includes: The second rotation data determination unit is used to determine the second rotation data of the robot from the previous moment to the current moment based on the output of the robot's inertial measurement unit. The second translation data determination unit is used to determine the second translation data of the robot from the previous time to the current time based on the output of the robot's ultra-wideband tag. The second pose prediction unit is used to predict the pose of the robot at the current moment based on the second rotation data and the second translation data, and obtain the second predicted pose. The second positioning optimization data determination unit is used to determine the second predicted pose as the positioning optimization data.

[0087] Optionally, the positioning module 1003 includes: The first point cloud registration unit is used to perform point cloud registration based on the above target point cloud data and the above environmental point cloud map to obtain the first original positioning result. The first optimization unit is used to perform nonlinear optimization on the above-mentioned positioning optimization data and the above-mentioned first original positioning result based on a preset error equation to obtain the above-mentioned positioning result.

[0088] Optionally, the positioning module 1003 includes: The second point cloud registration unit is used to perform point cloud registration based on the above target point cloud data and the above environmental point cloud map to obtain the second original positioning result. The second optimization unit is used to fuse the above-mentioned positioning optimization data and the above-mentioned second original positioning result based on a preset lossless Kalman filter to obtain the above-mentioned positioning result.

[0089] Optionally, the aforementioned preset environmental point cloud map is an environmental point cloud map with two or more layers, and the resolution of the uppermost environmental point cloud map is lower, while the resolution of the lowermost environmental point cloud map is higher; the aforementioned second point cloud registration unit includes: The current map is determined by a sub-unit, which is used to determine one environmental point cloud map as the current environmental point cloud map in the above two or more environmental point cloud maps in order of resolution from low to high. The current map point cloud registration subunit is used to perform point cloud registration based on the target point cloud data and the current environment point cloud map if the current environment point cloud map is the top layer, thereby obtaining the second original positioning result of the current environment point cloud map; if the current environment point cloud map is not the top layer, the optimized positioning result of the previous layer environment point cloud map is used as the initial value for point cloud registration, and point cloud registration is performed based on the target point cloud data and the current environment point cloud map to obtain the second original positioning result of the current environment point cloud map. The aforementioned second optimization unit includes: The current map positioning optimization subunit is used to fuse the above positioning optimization data and the second original positioning result of the above current environment point cloud map based on a preset lossless Kalman filter to obtain the optimized positioning result of the above current environment point cloud map. The positioning result determination sub-unit is used to determine the optimized positioning result of the current environment point cloud map as the positioning result if the current environment point cloud map is the lowest level.

[0090] Optionally, the determining module 1002 mentioned above includes: The historical positioning result acquisition unit is used to acquire the positioning results obtained by the robot based on the lidar at a historical moment. The third positioning optimization data determination unit is used to determine the positioning optimization data for the current moment based on the positioning results at the aforementioned historical moments.

[0091] Optionally, the aforementioned historical time is the N times preceding the current time, where N is a positive integer; the aforementioned third localization optimization data determination unit is specifically used to determine the localization results of the aforementioned N times as the localization optimization data at the current time if N is 1, and to perform polynomial fitting on the pose vector based on the localization results of the aforementioned N times, and to predict the pose of the robot at the current time based on the fitting result, thereby obtaining a third prediction result, and determining the aforementioned third prediction result as the localization optimization data at the current time.

[0092] Optionally, the positioning module 1003 includes: The third point cloud registration unit is used to perform point cloud registration based on the above-mentioned target point cloud data and the above-mentioned environmental point cloud map, using the above-mentioned positioning optimization data as the initial value for point cloud registration, to obtain the above-mentioned positioning result.

[0093] Optionally, the determining module 1002 mentioned above includes: The translation initial value determination unit is used to determine the initial value of the predicted translation of the robot based on the output of the ultra-wideband tag of the robot. The rotation initial value determination unit is used to determine the initial value of the predicted rotation of the robot, wherein the initial value of the predicted rotation includes at least two specified rotation angles. The fourth positioning optimization data determination unit is used to determine the initial values ​​of the predicted translation and the initial values ​​of the predicted rotation as the positioning optimization data.

[0094] Optionally, the positioning module 1003 includes: The combination unit is used to combine the initial value of the predicted translation with each specified rotation angle in the initial value of the predicted rotation to obtain at least one set of positioning optimization data to be determined. The fourth point cloud registration unit is used to use each set of undetermined positioning optimization data as the initial value for point cloud registration, and to perform point cloud registration based on the above target point cloud data and the above environmental point cloud map to obtain at least one undetermined positioning result. The optimal result determination unit is used to determine the optimal positioning result among the above at least one positioning result to be determined as the positioning result of the robot.

[0095] As can be seen from the above, through the embodiments of this application, after the robot collects the raw point cloud data at the current moment using LiDAR, it preprocesses the raw point cloud data to obtain target point cloud data, and then determines the positioning optimization data at the current moment. The positioning optimization data is used to optimize the robot's positioning process. Finally, based on the target point cloud data, the positioning optimization data, and the preset environmental point cloud map, the robot's positioning result is obtained. A new parameter, positioning optimization data, is introduced into the robot's positioning process. This optimization data optimizes the robot's positioning process to ensure the accuracy and precision of the final positioning result.

[0096] This application also provides a robot; please refer to [link / reference]. Figure 11 The robot 11 in this embodiment includes: a memory 1101, and one or more processors 1102. Figure 11(Only one is shown in the image) and a computer program stored in memory 1101 and executable on the processor. Memory 1101 stores software programs and units. The processor 1102 executes various functional applications and data processing by running the software programs and units stored in memory 1101 to obtain resources corresponding to the aforementioned preset events. Specifically, the processor 1102 implements the various steps proposed in the method embodiments by running the aforementioned computer program stored in memory 1101, which will not be repeated here.

[0097] It should be understood that, in the embodiments of this application, the processor 1102 may be a CPU, but it may also be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor.

[0098] Memory 1101 may include read-only memory and random access memory, and provides instructions and data to processor 1102. Some or all of memory 1101 may also include non-volatile random access memory. For example, memory 1101 may also store device category information.

[0099] As can be seen from the above, through the embodiments of this application, after the robot collects the raw point cloud data at the current moment using LiDAR, it preprocesses the raw point cloud data to obtain target point cloud data, and then determines the positioning optimization data at the current moment. The positioning optimization data is used to optimize the robot's positioning process. Finally, based on the target point cloud data, the positioning optimization data, and the preset environmental point cloud map, the robot's positioning result is obtained. A new parameter, positioning optimization data, is introduced into the robot's positioning process. This optimization data optimizes the robot's positioning process to ensure the accuracy and precision of the final positioning result.

[0100] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the above device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this application. The specific working process of the units and modules in the above system can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0101] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0102] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of external device software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0103] In the embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the system embodiments described above are merely illustrative. For instance, the division of modules or units described above is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be an indirect coupling or communication connection between devices or units through some interfaces, and may be electrical, mechanical, or other forms.

[0104] The units described above as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0105] If the integrated units described above are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing associated hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable storage medium can include: any entity or device capable of carrying the computer program code, a recording medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer-readable storage device, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium, etc.

[0106] The above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A positioning method, characterized in that, The positioning method is applied to a robot and includes: After acquiring the raw point cloud data at the current moment through the robot's lidar, the raw point cloud data is preprocessed to obtain the target point cloud data; Determine the positioning optimization data at the current moment, and the positioning optimization data is used to optimize the robot's positioning process; Based on the order of resolution from low to high, one layer of environmental point cloud map is determined as the current environmental point cloud map in the preset environmental point cloud map. The preset environmental point cloud map is an environmental point cloud map with two or more layers, and the resolution of the uppermost environmental point cloud map is lower, while the resolution of the lowermost environmental point cloud map is higher. If the current environment point cloud map is the top layer, then the preset default value is used as the initial value for point cloud registration. Point cloud registration is performed based on the target point cloud data and the current environment point cloud map to obtain the original positioning result of the current environment point cloud map. If the current environmental point cloud map is not the top layer, the optimized positioning result of the previous layer environmental point cloud map is used as the initial value for point cloud registration. Point cloud registration is performed based on the target point cloud data and the current environmental point cloud map to obtain the original positioning result of the current environmental point cloud map. The optimized positioning data and the original positioning results of the current environment point cloud map are fused based on a preset lossless Kalman filter to obtain the optimized positioning result of the current environment point cloud map. If the current environment point cloud map is the lowest layer, then the optimized positioning result of the current environment point cloud map is determined as the positioning result of the robot.

2. The positioning method as described in claim 1, characterized in that, Determining the positioning optimization data at the current moment includes: Based on the output of the robot's inertial measurement unit, the first rotation data and the first translation data of the robot from the previous moment to the current moment are determined; Based on the first rotation data and the first translation data, the pose of the robot at the current moment is predicted to obtain the first predicted pose. The first predicted pose is determined as the positioning optimization data.

3. The positioning method as described in claim 1, characterized in that, Determining the positioning optimization data at the current moment includes: Based on the output of the robot's inertial measurement unit, the second rotation data of the robot from the previous moment to the current moment is determined; Based on the output of the robot's ultra-wideband tag, determine the robot's second translation data from the previous moment to the current moment; Based on the second rotation data and the second translation data, the pose of the robot at the current moment is predicted to obtain a second predicted pose. The second predicted pose is determined as the positioning optimization data.

4. A positioning device, characterized in that, The positioning device is used in a robot and includes: The extraction module is used to preprocess the raw point cloud data acquired by the robot's lidar at the current moment to obtain the target point cloud data. A determination module is used to determine the positioning optimization data at the current moment, and the positioning optimization data is used to optimize the positioning process of the robot; The positioning module is used to obtain the positioning result of the robot based on the target point cloud data, the positioning optimization data, and the preset environmental point cloud map; The positioning module includes: The current map determination sub-unit is used to determine one layer of environmental point cloud map as the current environmental point cloud map in order of resolution from low to high. The preset environmental point cloud map is an environmental point cloud map with two or more layers, and the resolution of the uppermost environmental point cloud map is lower, while the resolution of the lowermost environmental point cloud map is higher. The current map point cloud registration subunit is used to perform point cloud registration based on the target point cloud data and the current environment point cloud map, if the current environment point cloud map is the top layer, by using a preset default value as the initial value for point cloud registration, and obtaining the original positioning result of the current environment point cloud map; if the current environment point cloud map is not the top layer, by using the optimized positioning result of the previous layer environment point cloud map as the initial value for point cloud registration, and performing point cloud registration based on the target point cloud data and the current environment point cloud map, and obtaining the original positioning result of the current environment point cloud map. The current map positioning optimization subunit is used to fuse the positioning optimization data and the original positioning results of the current environment point cloud map based on a preset lossless Kalman filter to obtain the optimized positioning result of the current environment point cloud map. The positioning result determination subunit is used to determine the optimized positioning result of the current environment point cloud map as the positioning result of the robot if the current environment point cloud map is the lowest level.

5. A robot comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the method as described in any one of claims 1 to 3.

6. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1 to 3.

Images