Pose determination method, storage medium, electronic equipment and program product

Abstract

The invention provides a pose determination method, a storage medium, electronic equipment and a program product, and relates to the technical field of positioning. The pose method comprises the steps that if it is determined that the pose of the self-moving device is lost at a target moment, a plurality of particles are created, each particle has a first pose, and the first pose represents pose estimation of the self-moving device at the target moment; based on the plurality of particles, determining candidate particles meeting the first pose precision at the target moment; based on the candidate particles, target particles meeting second pose precision under multiple moving distances of the self-moving equipment are determined; and determining the current pose of the self-moving device based on the current pose of the target particle. According to the invention, the autonomous navigation capability of the self-moving device is improved, the frequency of stopping operation or manual intervention of the self-moving device is reduced, and the navigation intelligence and user experience of the self-moving device are improved.

Description

Technical Field

[0001] The present application relates to the field of positioning technology, and in particular to a posture determination method, a storage medium, an electronic device and a program product. Background Art

[0002] Position loss refers to the inability of a self-moving device to accurately determine its position and orientation in the environment during navigation. When a self-moving device loses its position, an alarm signal is usually triggered, causing the self-moving device to stop operating or requiring manual intervention to restore its position. These situations will reduce the navigation intelligence of the self-moving device, thereby affecting the user experience. Summary of the invention

[0003] In view of this, embodiments of the present application provide a posture determination method, a storage medium, an electronic device, and a program product.

[0004] In a first aspect, an embodiment of the present application provides a method for determining a posture, including: if it is determined that the posture of a self-moving device at a target time is lost, creating multiple particles, each particle having a first posture, and the first posture represents the posture estimate of the self-moving device at the target time; based on the multiple particles, determining candidate particles that meet the first posture accuracy at the target time; based on the candidate particles, determining target particles that meet the second posture accuracy at multiple moving distances of the self-moving device; based on the current posture of the target particle, determining the current posture of the self-moving device.

[0005] In combination with the first aspect, in certain implementations of the first aspect, based on multiple particles, candidate particles that meet the first pose accuracy at the target time are determined, including: mapping the multiple particles to a grid structure, the grid structure including multiple grids; based on the first poses of the particles contained in each of the multiple grids, determining the average particle corresponding to each of the multiple grids, the pose of the average particle is determined based on the average value of the first poses of the particles contained in the grids; based on the average particles corresponding to each of the multiple grids, determining the candidate particles that meet the first pose accuracy at the target time; preferably, at the target time, the multiple particles present a Gaussian distribution around the optimal pose estimate of the self-moving device at the target time.

[0006] In combination with the first aspect, in certain implementations of the first aspect, based on the average particles corresponding to each of the multiple grids, a candidate particle that meets the first pose accuracy at the target time is determined, including: for the average particle corresponding to each grid, obtaining the first scanning data collected by the laser radar under the average particle; mapping the laser points contained in the first scanning data to the target grid map to obtain the position of each laser point in the target grid map; based on the positions of each laser point contained in the first scanning data in the target grid map, calculating the matching score of each laser point contained in the first scanning data; if the average value of the matching scores of the laser points contained in the first scanning data is greater than the first score threshold, determining that the average particle is a candidate particle that meets the first pose accuracy at the target time.

[0007] In combination with the first aspect, in certain implementations of the first aspect, before obtaining the first scanning data collected by the laser radar under the average particle, it also includes: obtaining the second scanning data collected by the laser radar under the average particle; matching the second scanning data with the target grid map to determine the transformation parameters when the match is successful; and calibrating the average particle based on the transformation parameters when the match is successful.

[0008] In combination with the first aspect, in certain implementations of the first aspect, the second scan data is matched with the target grid map to determine the transformation parameters when the match is successful, including: mapping the laser points contained in the second scan data to the target grid map to obtain the position of each laser point in the target grid map; based on the positions of each laser point contained in the second scan data in the target grid map, determining the target point cloud corresponding to each laser point contained in the second scan data, the target point cloud corresponding to the laser point is a point cloud located within the target range of the laser point; matching the target point cloud corresponding to each laser point contained in the second scan data with the second scan data to determine the transformation parameters when the match is successful.

[0009] In combination with the first aspect, in some implementations of the first aspect, based on the positions of the laser points included in the first scanning data in the target grid map, the matching scores of the laser points included in the first scanning data are calculated, including: if the laser point falls into an occupied area in the target grid map, the matching score of the laser point is a first value; if the laser point falls into an idle area in the target grid map, the matching score of the laser point is a second value; if the laser point falls into an unknown area in the target grid map, the target obstacle point closest to the laser point is determined in the target grid map; if the distance between the laser point and the target obstacle point is less than a target distance threshold, Then the matching score of the laser point is the target multiple of the distance; if the distance between the laser point and the target obstacle point is greater than or equal to the target distance threshold, the matching score of the laser point is the third value; preferably, the second value is less than the first value, and the first value is less than the target multiple of the distance; the target multiple of the distance is less than or equal to the third value; preferably, the sum of the matching scores of the laser points contained in the first scanning data is obtained by subtracting the target multiple of the distance and / or the third value from the sum of the first value and / or the second value; the average value of the matching scores of the laser points contained in the first scanning data is obtained by dividing the sum of the matching scores by the number of laser points.

[0010] In combination with the first aspect, in certain implementations of the first aspect, based on candidate particles, target particles that meet the second posture accuracy at multiple moving distances of the self-moving device are determined, including: based on the multiple moving distances of the self-moving device, calculating the second posture of each candidate particle at each moving distance; calculating the matching score of the second posture of each candidate particle at each moving distance with the target grid map; and determining the candidate particles whose maximum value of the matching scores between the second posture at multiple moving distances and the target grid map is greater than a second score threshold, and / or the candidate particles whose matching scores between the second posture at multiple moving distances and the target grid map are greater than a third score threshold as target particles that meet the second posture accuracy.

[0011] In the second aspect, an embodiment of the present application provides a posture determination device, including: a creation module, which is used to create multiple particles if it is determined that the posture of the self-moving device at the target time is lost, each particle has a first posture, and the first posture represents the posture estimate of the self-moving device at the target time; a first determination module, which is used to determine candidate particles that meet the first posture accuracy at the target time based on the multiple particles; a second determination module, which is used to determine target particles that meet the second posture accuracy at multiple moving distances of the self-moving device based on the candidate particles; and a third determination module, which is used to determine the current posture of the self-moving device based on the current posture of the target particle.

[0012] In a third aspect, an embodiment of the present application provides a computer-readable storage medium, which stores a computer program for executing the posture determination method described in the first aspect.

[0013] In a fourth aspect, an embodiment of the present application provides an electronic device, comprising: a processor; a memory for storing processor executable instructions; the processor is used to execute the posture determination method described in the first aspect.

[0014] In a fifth aspect, an embodiment of the present application provides a computer program product, which includes instructions, which, when executed on an electronic device, enable the electronic device to implement the posture determination method described in the first aspect.

[0015] In this application, when the posture loss is detected, multiple particles are created, each particle represents a possible posture estimate of the self-mobile device, which can provide multiple potential posture options for the self-mobile device in the uncertain situation of posture loss, thereby increasing the possibility of finding the correct posture. Then, based on multiple particles, candidate particles that meet the first posture accuracy are screened out. This process effectively narrows the scope of posture estimation and improves the accuracy of posture estimation. Then, this scheme also considers the posture accuracy of candidate particles at different moving distances of the self-mobile device, and then determines the target particles that meet the second posture accuracy to ensure the stability of the posture estimation of the self-mobile device in continuous motion. Finally, based on the current posture of the target particle, the current posture of the self-mobile device is accurately determined. The whole process does not require human intervention, which improves the autonomous navigation capability of the self-mobile device, enables the self-mobile device to more effectively deal with the situation of posture loss, reduces the frequency of the self-mobile device stopping operation or manual intervention, and significantly improves the navigation intelligence and user experience of the self-mobile device. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] By describing the embodiments of the present application in more detail in conjunction with the accompanying drawings, the above and other purposes, features and advantages of the present application will become more apparent. The accompanying drawings are used to provide a further understanding of the embodiments of the present application and constitute a part of the specification. Together with the embodiments of the present application, they are used to explain the present application and do not constitute a limitation of the present application. In the accompanying drawings, the same reference numerals generally represent the same components or steps.

[0017] Figure 1 Shown is a positioning comparison schematic diagram provided in an embodiment of the present application.

[0018] Figure 2 The figure is a flow chart of a method for determining a posture according to an embodiment of the present application.

[0019] Figure 3The figure is a schematic diagram of a process for determining candidate particles that meet the first pose accuracy at a target time, provided by an embodiment of the present application.

[0020] Figure 4 The figure is a schematic diagram of a process for determining candidate particles that meet the first pose accuracy at a target time, provided by an embodiment of the present application.

[0021] Figure 5 The figure is a schematic diagram of a process for determining candidate particles that meet the first pose accuracy at a target time, provided by another embodiment of the present application.

[0022] Figure 6 The figure is a schematic diagram of a process for determining target particles that meet a second posture accuracy at multiple moving distances of a self-moving device, provided by an embodiment of the present application.

[0023] Figure 7 Shown is a schematic structural diagram of a posture determination device provided in one embodiment of the present application.

[0024] Figure 8 Shown is a schematic structural diagram of an electronic device provided in one embodiment of the present application. DETAILED DESCRIPTION

[0025] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0026] Figure 1 FIG. 1 is a schematic diagram of positioning comparison provided by an embodiment of the present application. Figure 1 As shown in a, b, and c in the figure, in the grid map, the wall information is marked in black, and the obstacle information is marked in red boundary points. When the posture of the self-moving device is correct, the obstacle information and the wall information should mostly overlap. However, in a, b, and c, some obstacle information does not overlap with the wall information, that is, the posture of the self-moving device is not completely accurate.

[0027] In practical applications, a matching threshold is usually set to determine whether the self-moving device has lost its position. When the self-moving device loses its position, an alarm signal is usually triggered, causing the self-moving device to stop operating or requiring manual intervention to restore its position. This situation will reduce the navigation intelligence of the self-moving device and thus affect the user experience.

[0028] Based on the above, this application is proposed. Figure 2FIG. 1 is a flow chart of a method for determining a posture according to an embodiment of the present application. For example, the method is applied to a self-propelled device equipped with a laser radar. Figure 2 As shown, the method includes the following steps.

[0029] Step S210: If it is determined that the position and posture of the mobile device at the target time is lost, multiple particles are created.

[0030] Each particle has a first pose, which represents the pose estimate of the self-moving device at the target time, that is, the position and direction that the self-moving device may be at the target time.

[0031] In some embodiments, multiple particles are Gaussian distributed around the optimal pose estimate of the self-moving device at the target time. That is, each particle is a random sample, which is randomly distributed on a probability distribution, and this distribution is a Gaussian distribution, which means that particles are more likely to gather near the optimal pose estimate of the self-moving device at the target time, and the density of particles gradually decreases as the distance increases.

[0032] For example, the number of particles is 21×21, and the coordinates of each particle satisfy: x~N(x o ,cov x ), y~N(y o ,cov y ), yaw~N(yaw o ,cov yaw ). Where N represents Gaussian distribution, (x o ,y o ,yaw o ) represents the optimal pose estimation of the mobile device at the target time, cov x =cov y =0.02, cov yaw =0.01.

[0033] By generating Gaussian distributed particles around the optimal pose estimate, the randomness and error of pose estimation can be simulated more naturally. Specifically, due to the characteristics of Gaussian distribution, most particles will be concentrated in the high probability area of ​​the optimal pose estimate, while fewer particles will be distributed at farther locations, improving the accuracy and robustness of pose estimation.

[0034] Step S220, based on multiple particles, determine the candidate particles that meet the first pose accuracy at the target time.

[0035] The first pose accuracy refers to a standard for the accuracy or error range of the pose estimation of the self-mobile device at the target time. For example, the accuracy of the pose estimation represented by the first pose of each particle at the target time is evaluated, and only those particles whose errors are within the preset first pose accuracy will be selected as candidate particles.

[0036] Step S230, based on the candidate particles, determine the target particles that meet the second posture accuracy at multiple moving distances of the self-moving device.

[0037] Specifically, the accuracy of the pose estimation represented by the candidate particles after they move a certain distance from the mobile device is evaluated, and then the target particles are determined from the candidate particles.

[0038] Step S240, based on the current posture of the target particle, determine the current posture of the mobile device.

[0039] It is understandable that the target particle represents the optimal pose estimate of the self-moving device. Therefore, in some embodiments, if there is one target particle, the current pose of the target particle is directly determined as the current pose of the self-moving device. If there are multiple target particles, a comprehensive pose estimate is calculated based on the current pose of the target particle and their respective weights. This comprehensive pose estimate is the current pose of the self-moving device.

[0040] In some embodiments, after the mobile device successfully recovers the current position, it automatically returns to the normal positioning state and clears all particles. If there is no target particle, the process from step S210 to step S230 is repeated until a target particle that meets the conditions is found.

[0041] In this embodiment, when the posture loss is detected, multiple particles are created, each particle represents a possible posture estimation of the self-mobile device, which can provide multiple potential posture options for the self-mobile device in the uncertain situation of posture loss, thereby increasing the possibility of finding the correct posture. Then, based on multiple particles, candidate particles that meet the first posture accuracy are screened out. This process effectively narrows the scope of posture estimation and improves the accuracy of posture estimation. Then, this scheme also considers the posture accuracy of candidate particles at different moving distances of the self-mobile device, and then ensures the stability of posture estimation of the self-mobile device in continuous motion by determining the target particles that meet the second posture accuracy. Finally, based on the current posture of the target particle, the current posture of the self-mobile device is accurately determined. The whole process does not require human intervention, which improves the autonomous navigation capability of the self-mobile device, enables the self-mobile device to more effectively deal with the situation of posture loss, reduces the frequency of stopping operation or manual intervention of the self-mobile device, and significantly improves the navigation intelligence and user experience of the self-mobile device.

[0042] Figure 3 FIG. 1 is a flow chart of determining candidate particles that meet the first pose accuracy at the target time according to an embodiment of the present application. Figure 2 Based on the embodiment shown, Figure 3 The embodiment shown is described below in detail. Figure 3 The embodiment shown and Figure 2 The differences and similarities of the illustrated embodiments are not described in detail.

[0043] like Figure 3 As shown, in this embodiment, based on multiple particles, determining the candidate particles that meet the first pose accuracy at the target time includes the following steps.

[0044] Step S310, mapping multiple particles to a grid structure.

[0045] Specifically, the grid structure includes a plurality of grids, each grid corresponding to a small area in the pose space of the self-moving device, and the pose space is composed of three dimensions of the position (x and y coordinates) and direction (yaw) of the self-moving device.

[0046] In some embodiments, in order to achieve mapping, the resolution of the grid structure is first defined, that is, the size of the grid in each dimension. Exemplarily, in this embodiment, the resolution of the x and y dimensions is 0.2, and the resolution of the yaw dimension is 0.1. Then, each particle is assigned to the corresponding grid according to its first pose.

[0047] Step S320: determining the average particle corresponding to each of the plurality of grids based on the first position of the particles contained in each of the plurality of grids.

[0048] The pose of the average particle is determined based on the average of the first poses of the particles contained in the grid. Specifically, for each grid, the first poses of all particles in the grid are collected, including the position of the particles in the x and y coordinates and the angle in the yaw direction. Then, the x coordinate values, y coordinate values ​​and angle values ​​in the yaw direction of these particles are arithmetic averaged to obtain an average x coordinate, an average y coordinate and an average yaw angle. These three average values ​​together constitute the pose of the average particle in the grid.

[0049] Step S330, based on the average particles corresponding to each of the multiple grids, determine the candidate particles that meet the first pose accuracy at the target time.

[0050] Specifically, the position and posture of each average particle are compared with known environmental information or other reference information, and the error between them is calculated. If the error corresponding to the position and posture of an average particle is within the range allowed by the first position accuracy, then this average particle meets the accuracy requirements and can be determined as a candidate particle at the target moment. These candidate particles represent the possible true position and posture of the mobile device at the target moment.

[0051] In this embodiment, the particles are mapped to the grid structure, so that the original continuous posture space is discretized into a limited number of grids, each grid contains a certain number of particles, and the operation of a single particle can be converted into the operation of the grid, which simplifies the processing of the particles and reduces the complexity of the calculation. Secondly, the size of the grid can be dynamically adjusted according to the distribution range of the first posture of the particle, which can ensure that all particles are included in the corresponding grid, avoid the generation of too many redundant grids, and reduce the memory usage. This dynamic adjustment mechanism can more flexibly adapt to the distribution of particles in different situations and further optimize the use of storage resources. In addition, the number of particles in each grid is relatively small. By calculating the average particle in each grid instead of processing all particles in the entire space, the amount of calculation is greatly reduced and the efficiency of the algorithm is improved. In addition, the posture of the average particle can be regarded as a comprehensive representation of the posture estimation of all particles in the grid. In this way, the particle information dispersed in the grid can be summarized into a more representative average particle, providing a basis for the next step of screening candidate particles that meet the first posture accuracy requirements.

[0052] Figure 4 FIG. 1 is a flow chart of determining candidate particles that meet the first pose accuracy at the target time according to an embodiment of the present application. Figure 3 Based on the embodiment shown, Figure 4 The embodiment shown is described below in detail. Figure 4 The embodiment shown and Figure 3 The differences and similarities of the illustrated embodiments are not described in detail.

[0053] like Figure 4 As shown, in this embodiment, based on the average particles corresponding to each of the multiple grids, determining the candidate particles that meet the first pose accuracy at the target time includes the following steps.

[0054] Step S410, for the average particle corresponding to each grid, obtain the first scanning data collected by the laser radar under the average particle.

[0055] Exemplarily, at the position corresponding to the average particle, a laser radar is used to emit laser beams, and the time it takes for these laser beams to be reflected back after encountering surrounding obstacles is measured, so as to calculate the distance between the mobile device and the obstacles. These measurement results form a set of point cloud data, namely, the first scanning data, which represents the outline and layout of the surrounding environment of the mobile device at the position corresponding to the average particle.

[0056] Step S420, mapping the laser points included in the first scanning data to the target grid map to obtain the position of each laser point in the target grid map.

[0057] Exemplarily, the laser points included in the first scanning data are converted from a laser radar coordinate system to a map coordinate system corresponding to the target grid map to obtain the position of each laser point in the target grid map.

[0058] Step S430: Calculate the matching score of each laser point included in the first scan data based on the position of each laser point included in the first scan data in the target grid map.

[0059] In some embodiments, if the laser point falls into an occupied area in the target grid map, the matching score of the laser point is a first value; if the laser point falls into an idle area in the target grid map, the matching score of the laser point is a second value; if the laser point falls into an unknown area in the target grid map, the target obstacle point closest to the laser point is determined in the target grid map; if the distance between the laser point and the target obstacle point is less than a target distance threshold, the matching score of the laser point is a target multiple of the distance; if the distance between the laser point and the target obstacle point is greater than or equal to the target distance threshold, the matching score of the laser point is a third value.

[0060] Specifically, laser points that fall into occupied areas indicate that the laser points are correctly identified and matched with obstacles on the target grid map. Laser points that fall into idle areas indicate that the laser points may have hit dynamic obstacles. Laser points that fall into unknown areas indicate that there is not enough information to determine that they match obstacles in the target grid map.

[0061] When the laser point falls into an unknown area in the target grid map, the matching of the laser point cannot be directly judged due to the lack of clear obstacle information in the unknown area. In order to further evaluate the matching scores of these laser points, the target obstacle point closest to the laser point will be found, and the matching score will be determined based on the distance between them. Furthermore, if the distance between the laser point and the target obstacle point is less than the target distance threshold, it means that the laser point may be close to an actual obstacle, but because it falls in an unknown area, its accuracy cannot be fully determined, so a matching score proportional to the distance is assigned. When the distance between the laser point and the target obstacle point is greater than or equal to the target distance threshold, the correlation between the laser point and the known obstacle is lower and the uncertainty of the matching is greater, so a fixed third value is assigned as a penalty item.

[0062] Optionally, the second value is less than the first value, the first value is less than the target multiple of the distance, and the target multiple of the distance is less than or equal to the third value. For example, the first value is 1, the second value is 0, the target distance threshold is 0.5, the target multiple is 10, and the third value is 5.

[0063] In theory, the laser point should hit the obstacle and reflect back, so it is more likely to appear in the occupied area. Therefore, the second value is smaller than the first value, that is, the laser point that falls into the free area is less consistent with the behavior pattern of the laser point than the laser point that falls into the occupied area. In addition, the target multiple of the distance is less than or equal to the third value because when the distance between the laser point and the target obstacle point exceeds the target distance threshold, the correlation between the laser point and the obstacle is lower and the uncertainty of the match is greater. Therefore, it is necessary to increase the penalty to reduce the impact of these laser points on the success of the match. Through such a matching score setting, the matching quality of the laser points in different situations can be evaluated more accurately.

[0064] Step S440: If the average value of the matching scores of the laser points included in the first scanning data is greater than the first score threshold, the average particle is determined to be a candidate particle that meets the first pose accuracy at the target time.

[0065] In some embodiments, the sum of the matching scores of the laser points included in the first scanning data is obtained by subtracting the target multiple of the distance and / or the third value from the sum of the first value and / or the second value; and the average value of the matching scores of the laser points included in the first scanning data is obtained by dividing the sum of the matching scores by the number of laser points.

[0066] Exemplarily, following the above example, a laser point that falls into an occupied area is assigned 1 point, and a laser point that falls into an idle area is assigned 0 points. At the same time, for laser points that fall into unknown areas, find their nearest target obstacle points in the target grid map and calculate the distance. If the distance between a laser point and the target obstacle point is 0.4, which is less than the target distance threshold of 0.5, then the matching score of the laser point is 10 times 0.4, that is, 4 points; and if the distance between another laser point and the target obstacle point is 0.6 meters, which is greater than or equal to the target distance threshold of 0.5, then its matching score is a fixed 5 points.

[0067] Next, add up the matching scores of all laser points in the first scan data. Assume that there are 20 laser points in total, 16 of which are in the occupied area, with a matching score of 16 points; 3 are in the free area, with a matching score of 0 points; and 1 is in the unknown area and is far away, with a matching score of 5 points. Add up these matching scores, and the total matching score is 16-5=11 points. Then, divide the total matching score of 11 by the number of laser points 20, and the average matching score is 0.55 points.

[0068] If the preset first score threshold is 0.5 points, then since 0.55 points is greater than 0.5 points, it can be determined that the position and posture of the average particle at the target time meets the first position accuracy requirement, and it is used as a candidate particle.

[0069] This scheme calculates the respective matching scores based on the positions of the laser points in the grid map, and evaluates the matching degree between the laser points and the target grid map in a quantitative way, providing a quantitative basis for the pose estimation of the average particle. In addition, the positive contribution and negative penalty of the matching between the laser points and the target grid map are reasonably considered, so that the final sum of the matching scores can fully reflect the overall matching quality between the first scan data and the target grid map. Then, by dividing the sum of the matching scores by the number of laser points, the average value obtained can represent the average contribution of a single laser point to the pose estimation. If this average value is greater than the preset first score threshold, it means that on the whole, the matching between the laser points in the first scan data and the target grid map is good, and the pose estimation of the average particle is relatively accurate, so it can be regarded as a candidate particle that meets the first pose accuracy.

[0070] Figure 5 FIG. 1 is a flow chart of determining candidate particles that meet the first pose accuracy at the target time according to another embodiment of the present application. Figure 4 Based on the embodiment shown, Figure 5 The embodiment shown is described below in detail. Figure 5 The embodiment shown and Figure 4 The differences and similarities of the illustrated embodiments are not described in detail.

[0071] like Figure 5 As shown, in this embodiment, before obtaining the first scanning data collected by the laser radar under the average particle, the following steps are also included.

[0072] Step S510, obtaining second scanning data collected by the laser radar under the average particle.

[0073] Exemplarily, the method in step S410 may be referred to to obtain the second scanning data collected by the laser radar under the average particle, which will not be described in detail here.

[0074] Step S520, matching the second scan data with the target grid map, and determining the transformation parameters when the matching is successful.

[0075] Specifically, the transformation parameters include a translation vector and a rotation matrix. The translation vector is a three-dimensional vector that indicates how far the second scan data needs to move in three-dimensional space to accurately align with the corresponding point on the target grid map; the rotation matrix is ​​used to represent the rotation in three-dimensional space to ensure that the direction of the second scan data is consistent with the direction on the target grid map.

[0076] In some embodiments, each point in the second scan data is compared with a corresponding grid in the target grid map, including detecting whether a peak in the second scan data coincides with an obstacle position in the target grid map, and evaluating whether an overall shape of the second scan data is consistent with a structural feature in the target grid map.

[0077] In other embodiments, the laser points contained in the second scanning data are mapped to the target grid map to obtain the position of each laser point in the target grid map; based on the positions of each laser point contained in the second scanning data in the target grid map, the target point clouds corresponding to each laser point contained in the second scanning data are determined; the target point clouds corresponding to each laser point contained in the second scanning data are matched with the second scanning data to determine the transformation parameters when the match is successful.

[0078] Specifically, mapping laser points to the target grid map means converting the laser points in the three-dimensional space collected by the laser radar and corresponding them to the pre-established target grid map. In this process, each laser point is assigned to the corresponding cell on the target grid map according to its coordinates in the laser radar coordinate system, and the position of each laser point in the target grid map is obtained.

[0079] Furthermore, the target point cloud corresponding to the laser point is a point cloud within the target range of the laser point. Specifically, for each laser point converted to the target grid map, the map point cloud within the target range (e.g., 0.5 meters) in the target grid map is queried, and these map point clouds are the target point clouds. In this way, accurate source points and target points can be provided for the subsequent iterative closest point algorithm, thereby achieving calibration of the pose of the average particle. Specifically, the iterative closest point algorithm uses this point cloud information and finds the optimal pose transformation through multiple iterative calculations to minimize the difference between the laser point cloud and the target point cloud.

[0080] This scheme can more accurately capture the correspondence between the laser point and the environmental features in the target grid map by determining the target point cloud corresponding to each laser point in the target grid map. Then, the target point cloud is matched with the second scan data, which is equivalent to calibrating the position and posture of the average particle based on the known environmental information. In addition, the matching process is carried out within a limited range of the target point cloud, avoiding large-scale search on the entire target grid map and reducing the amount of calculation.

[0081] Step S530, calibrating the average particle based on the transformation parameters when the matching is successful.

[0082] Assume that the transformation parameters indicate that the position corresponding to the average particle is offset by 1 meter to the north and 0.5 meter to the west relative to the actual environment, and needs to be rotated 5° counterclockwise. The process of calibrating the position corresponding to the average particle using these transformation parameters is: move the position corresponding to the average particle 1 meter to the north, 0.5 meter to the west, and then rotate 5° counterclockwise.

[0083] In this embodiment, by acquiring the second scan data under the average particle, the latest information about the environment around the mobile device can be collected, reflecting the actual obstacle distribution around the current position of the mobile device. Then, the second scan data is matched with the target grid map. If the match is successful, the average particle is calibrated based on the transformation parameters, that is, the average particle is adjusted to a posture closer to the actual environment, thereby improving the accuracy of positioning.

[0084] Figure 6 The figure shows a flow chart of determining target particles that meet the second position accuracy under multiple moving distances of the self-moving device provided by an embodiment of the present application. Figure 2 Based on the embodiment shown, Figure 6 The embodiment shown is described below in detail. Figure 6 The embodiment shown and Figure 2 The differences and similarities of the illustrated embodiments are not described in detail.

[0085] like Figure 6As shown, in this embodiment, based on candidate particles, determining target particles that meet the second posture accuracy at multiple moving distances of the self-moving device includes the following steps.

[0086] Step S610, based on multiple moving distances of the self-moving device, calculate the second pose of each candidate particle at each moving distance.

[0087] Exemplarily, first, the pose change T(odo) provided by the odometer, including position change (x_odo, y_odo) and angle change (yaw_odo), is used to predict the second pose of each candidate particle at each moving distance. Specifically, the pose T(t) of the candidate particle is multiplied by the odometer pose change T(odo) to obtain the second pose T(t+1) of the candidate particle at a certain moving distance.

[0088] In addition, in the prediction process, in order to simulate the uncertainty in the real world, different noises are added to the odometer changes of each candidate particle. These noises are realized by the position noise coefficient a and the angle noise coefficient b. For example, the value range of a is -1.2 to 1.2, and the value range of b is -1.1 to 1.1. A value is taken at a certain interval to form multiple pairs of combinations and assigned to each candidate particle. Among them, the interval depends on the number of particles. If the number of particles is 21×21, then the interval of a is 2.4 / 20 and the interval of b is 2.2 / 20. In this way, each candidate particle will be affected by different noises during prediction, resulting in different prediction results of the second pose. In addition, on the basis of the second pose prediction, Gaussian noise is added to further simulate the randomness and error of the second pose prediction.

[0089] In this way, a series of possible second poses can be generated for each candidate particle at different moving distances, providing a rich data basis for subsequent pose accuracy evaluation and target particle determination.

[0090] Step S620, calculating the matching score between the second posture of each candidate particle at each moving distance and the target grid map.

[0091] For example, reference Figure 4 The method in the illustrated embodiment calculates the matching score between the second pose of each candidate particle at each moving distance and the target grid map, which will not be elaborated in detail here.

[0092] Step S630, the candidate particles whose maximum value of the matching scores between the second posture and the target grid map at multiple moving distances is greater than the second score threshold, and / or the candidate particles whose matching scores between the second posture and the target grid map at multiple moving distances are greater than the third score threshold, are determined as target particles that meet the second posture accuracy.

[0093] Optionally, the second score threshold is greater than the first score threshold. For example, the first score threshold is 0.5, and the second score threshold is 0.8.

[0094] Optionally, a plurality of the matching scores of the second posture and the target grid map under multiple moving distances refers to the matching scores of the second posture and the target grid map under the moving distances of the continuous target number. The number of continuous targets here is related to the total number of moving distances, for example, the number of targets is 80% of the total number of moving distances. Assuming that the total number of moving distances is 10 times, each moving 0.5 meters, it is necessary that the matching scores of the second posture and the target grid map under 8 consecutive moving distances are greater than the third score threshold before the candidate particle is considered to be a target particle that meets the second posture accuracy. Optionally, the third score threshold is the same as the first score threshold.

[0095] In this embodiment, first, by calculating the second posture of each candidate particle at different moving distances, the posture change of the self-moving device during continuous motion is simulated, which helps to capture the real motion trajectory of the self-moving device in a dynamic environment. Next, the matching score of each second posture and the target grid map is calculated to quantify the matching degree between the posture prediction result and the real target grid map. Then, by comparing the maximum value of the matching score with the second score threshold, and whether the matching score is greater than the third score threshold, the target particles that show high matching accuracy at different moving distances are screened out. This screening method comprehensively considers the local and overall performance of the candidate particles, ensuring the stability and consistency of the final target particles in continuous motion.

[0096] In some embodiments, before determining that the position and posture of the self-moving device at the target moment is lost, the initial position and posture of the self-moving device must be determined; the self-moving device is positioned based on the initial position and the position and posture of the self-moving device at the target moment during the movement is obtained; and it is determined whether the position and posture of the self-moving device at the target moment is lost.

[0097] Specifically, when the self-mobile device starts working, in order to achieve accurate positioning of the initial pose, the starting pose of the self-mobile device is first used as the initial pose, and the point cloud matching scheme is used to calibrate it. In addition, since the iterative closest point algorithm is sensitive to the accuracy of the initial pose, when there is a large deviation in the initial pose, the iterative closest point algorithm may not be able to find the correct position. In order to solve this problem, the self-mobile device sets a series of shadow poses near the input initial pose, and these shadow poses are evenly distributed around the initial pose. Then, each shadow pose is used as the initial pose, and the iterative closest point matching process is performed. The initial pose is calibrated according to the transformation parameters when the match is successful to obtain the initial calibration pose. Then, the matching degree of each initial calibration pose with the target grid map is calculated, and the initial calibration pose with the best matching degree is selected as the final initial pose of the self-mobile device.

[0098] The final initial pose provides a starting reference position and direction for the self-moving device. On this basis, the self-moving device uses the odometer data to predict its pose changes in the next movement. After each prediction, the self-moving device converts the point cloud data in the lidar coordinate system to the map coordinate system of the target grid map, and searches for the target point cloud corresponding to these point cloud data on the target grid map. Then, the predicted pose is calibrated by the iterative closest point algorithm to obtain the calibrated pose, that is, the precise pose of the self-moving device at the target time.

[0099] In order to determine whether the precise position and posture of the mobile device at the target moment is lost, first, the laser points in the scan data under the calibrated position and posture of the target moment are mapped to the target grid map, and the matching score of the laser points is calculated. If the matching score is greater than the preset threshold, it is considered that the position and posture of the target moment is not lost; otherwise, it is considered that the position and posture of the target moment is lost.

[0100] It should be noted that the matching score of the laser point and the method of calibrating the posture in this embodiment can refer to the above-mentioned embodiment. In this way, the positioning accuracy of the self-moving device during movement can be effectively evaluated, and the problem of positioning loss can be discovered and handled in time, thereby ensuring the navigation performance of the self-moving device.

[0101] Combination of the above Figures 1 to 6 , describes in detail the embodiment of the posture determination method of the present application, and the following is combined with Figure 7 , describes in detail the embodiment of the posture determination device of the present application. It should be understood that the description of the posture determination method embodiment corresponds to the description of the posture determination device embodiment, so the part not described in detail can refer to the previous method embodiment.

[0102] Figure 7FIG. 1 is a schematic diagram of the structure of a posture determination device provided by an embodiment of the present application. Figure 7 As shown, the posture determination device 70 provided in the embodiment of the present application includes:

[0103] A creation module 710, for creating a plurality of particles if it is determined that the position and posture of the self-moving device at the target time is lost, each particle having a first position, the first position representing the estimated position and posture of the self-moving device at the target time;

[0104] A first determination module 720 is used to determine, based on multiple particles, a candidate particle that satisfies the first pose accuracy at a target time;

[0105] A second determination module 730 is used to determine, based on the candidate particles, target particles that meet a second position and posture accuracy at multiple moving distances of the self-moving device;

[0106] The third determination module 740 is used to determine the current position and posture of the self-moving device based on the current position and posture of the target particle.

[0107] In one embodiment, the first determination module 720 is also used to map multiple particles to a grid structure, the grid structure includes multiple grids; based on the first position of the particles contained in each of the multiple grids, determine the average particle corresponding to each of the multiple grids, the position of the average particle is determined based on the average value of the first position of the particles contained in the grids; based on the average particle corresponding to each of the multiple grids, determine the candidate particles that meet the first position accuracy at the target time; preferably, at the target time, the multiple particles present a Gaussian distribution around the optimal position estimate of the self-moving device at the target time.

[0108] In one embodiment, the first determination module 720 is also used to obtain, for the average particle corresponding to each grid, the first scanning data collected by the laser radar under the average particle; map the laser points contained in the first scanning data to the target grid map to obtain the position of each laser point in the target grid map; calculate the matching score of each laser point contained in the first scanning data based on the position of each laser point contained in the first scanning data in the target grid map; if the average value of the matching scores of the laser points contained in the first scanning data is greater than the first score threshold, determine that the average particle is a candidate particle that meets the first pose accuracy at the target time.

[0109] In one embodiment, the first determination module 720 is also used to obtain second scanning data collected by the laser radar under the average particle; match the second scanning data with the target grid map to determine the transformation parameters when the match is successful; and calibrate the average particle based on the transformation parameters when the match is successful.

[0110] In one embodiment, the first determination module 720 is also used to map the laser points contained in the second scanning data to the target grid map to obtain the position of each laser point in the target grid map; based on the positions of each laser point contained in the second scanning data in the target grid map, determine the target point cloud corresponding to each laser point contained in the second scanning data, the target point cloud corresponding to the laser point is a point cloud located within the target range of the laser point; match the target point cloud corresponding to each laser point contained in the second scanning data with the second scanning data, and determine the transformation parameters when the match is successful.

[0111] In one embodiment, the first determination module 720 is further configured to: if the laser point falls into an occupied area in the target grid map, the matching score of the laser point is a first value; if the laser point falls into an idle area in the target grid map, the matching score of the laser point is a second value; if the laser point falls into an unknown area in the target grid map, a target obstacle point closest to the laser point is determined in the target grid map; if the distance between the laser point and the target obstacle point is less than a target distance threshold, the matching score of the laser point is a target multiple of the distance; if the distance between the laser point and the target obstacle point is greater than or equal to the target distance threshold, the matching score of the laser point is a third value; preferably, the second value is less than the first value, and the first value is less than the target multiple of the distance; the target multiple of the distance is less than or equal to the third value; preferably, the sum of the matching scores of the laser points included in the first scan data is obtained by subtracting the target multiple of the distance and / or the third value from the sum of the first value and / or the second value; and the average value of the matching scores of the laser points included in the first scan data is obtained by dividing the sum of the matching scores by the number of laser points.

[0112] In one embodiment, the second determination module 730 is also used to calculate the second posture of each candidate particle at each moving distance based on multiple moving distances of the self-moving device; calculate the matching score between the second posture of each candidate particle at each moving distance and the target grid map; and determine the candidate particles whose maximum value of the matching scores between the second posture at multiple moving distances and the target grid map is greater than a second score threshold, and / or the candidate particles whose matching scores between the second posture at multiple moving distances and the target grid map are greater than a third score threshold as target particles that meet the second posture accuracy.

[0113] Below, reference Figure 8 To describe an electronic device according to an embodiment of the present application. Figure 8 Shown is a schematic structural diagram of an electronic device provided by an exemplary embodiment of the present application.

[0114] like Figure 8 As shown, the electronic device 80 includes one or more processors 801 and a memory 802 .

[0115] The processor 801 may be a central processing unit (CPU) or other forms of processing units having data processing capabilities and / or instruction execution capabilities, and may control other components in the electronic device 80 to perform desired functions.

[0116] The memory 802 may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory may include, for example, random access memory (RAM) and / or cache memory (cache), etc. The non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 801 may run the program instructions to implement the posture determination method of each embodiment of the present application described above and / or other desired functions. Various contents such as multiple particles, candidate particles, target particles, and the current posture of target particles may also be stored in the computer-readable storage medium.

[0117] In one example, the electronic device 80 may further include: an input device 803 and an output device 804 , and these components are interconnected via a bus system and / or other forms of connection mechanisms (not shown).

[0118] The input device 803 may include, for example, a keyboard, a mouse, etc.

[0119] The output device 804 can output various information to the outside, including multiple particles, candidate particles, target particles, current positions of target particles, etc. The output device 804 can include, for example, a display, a speaker, a printer, a communication network and its connected remote output device, etc.

[0120] Of course, to simplify, Figure 8 Only some of the components related to the present application in the electronic device 80 are shown, and components such as a bus, an input / output interface, etc. are omitted. In addition, according to specific application situations, the electronic device 80 may also include any other appropriate components.

[0121] In addition to the above-mentioned methods and devices, an embodiment of the present application may also be a computer program product, which includes computer program instructions, which, when executed by a processor, enable the processor to execute the steps of the posture determination method according to various embodiments of the present application described above in this specification.

[0122] The computer program product may be written in any combination of one or more programming languages ​​to write program codes for performing the operations of the embodiments of the present application, including object-oriented programming languages, such as Java, C++, etc., and conventional procedural programming languages, such as "C" language or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as an independent software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0123] In addition, an embodiment of the present application may also be a computer-readable storage medium on which computer program instructions are stored. When the computer program instructions are executed by a processor, the processor executes the steps of the posture determination method according to various embodiments of the present application described above in this specification.

[0124] The computer readable storage medium can adopt any combination of one or more readable media. The readable medium can be a readable signal medium or a readable storage medium. The readable storage medium can include, for example, but is not limited to, a system, device or device of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination of the above. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0125] The basic principles of the present application are described above in conjunction with specific embodiments. However, it should be noted that the advantages, strengths, effects, etc. mentioned in the present application are only examples and not limitations, and it cannot be considered that these advantages, strengths, effects, etc. are required by each embodiment of the present application. In addition, the specific details disclosed above are only for the purpose of illustration and ease of understanding, not for limitation, and the above details do not limit the present application to being implemented by adopting the above specific details.

[0126] The block diagrams of the devices, apparatuses, equipment, and systems involved in this application are only illustrative examples and are not intended to require or imply that they must be connected, arranged, and configured in the manner shown in the block diagram. As will be appreciated by those skilled in the art, these devices, apparatuses, equipment, and systems can be connected, arranged, and configured in any manner. Words such as "including", "comprising", "having", etc. are open words, referring to "including but not limited to", and can be used interchangeably with them. The words "or" and "and" used here refer to the words "and / or" and can be used interchangeably with them, unless the context clearly indicates otherwise. The words "such as" used here refer to the phrase "such as but not limited to", and can be used interchangeably with them.

[0127] It should also be noted that in the apparatus, device and method of the present application, each component or each step can be decomposed and / or recombined. Such decomposition and / or recombination should be regarded as equivalent solutions of the present application.

[0128] The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use the present application. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein may be applied to other aspects without departing from the scope of the present application. Therefore, the present application is not intended to be limited to the aspects shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

[0129] The above description has been given for the purpose of illustration and description. In addition, this description is not intended to limit the embodiments of the present application to the forms disclosed herein. Although multiple example aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, changes, additions and sub-combinations thereof.

Claims

1. A method for determining a posture, characterized in that: Applied to a self-propelled device equipped with a laser radar, the method comprises: If it is determined that the position and posture of the self-moving device at the target time is lost, a plurality of particles are created, each particle having a first position, the first position representing an estimated position and posture of the self-moving device at the target time; Based on the multiple particles, determining a candidate particle that satisfies the first pose accuracy at the target time; Based on the candidate particles, determining target particles that meet a second posture accuracy at multiple moving distances of the self-moving device; Based on the current position and posture of the target particle, the current position and posture of the self-moving device is determined.

2. The method for determining a posture according to claim 1, characterized in that: Determining, based on the plurality of particles, a candidate particle that satisfies the first pose accuracy at the target time, comprising: mapping the plurality of particles to a grid structure, the grid structure comprising a plurality of grids; Based on the first position poses of the particles respectively contained in the plurality of grids, determining the average particle corresponding to each of the plurality of grids, wherein the position pose of the average particle is determined based on the average value of the first position poses of the particles contained in the grids; Determining, based on the average particles corresponding to each of the plurality of grids, candidate particles that satisfy the first posture accuracy at the target time; Preferably, at the target time, the plurality of particles present a Gaussian distribution around an optimal pose estimate of the self-moving device at the target time.

3. The method for determining a posture according to claim 2, characterized in that: The determining, based on the average particles corresponding to each of the plurality of grids, candidate particles that satisfy the first posture accuracy at the target time comprises: For each average particle corresponding to the grid, obtaining first scanning data collected by the laser radar under the average particle; Mapping the laser points included in the first scanning data to a target grid map to obtain a position of each of the laser points in the target grid map; Calculating a matching score of each laser point included in the first scanning data based on a position of each laser point included in the first scanning data in the target grid map; If the average value of the matching scores of the laser points included in the first scanning data is greater than a first score threshold, the average particle is determined to be a candidate particle that meets the first posture accuracy at the target time.

4. The method for determining a position and posture according to claim 3, characterized in that: Before obtaining the first scanning data collected by the laser radar under the average particle, the method further includes: Acquire second scanning data collected by the laser radar under the average particle; Matching the second scan data with the target grid map, and determining transformation parameters when the match is successful; The average particle is calibrated based on the transformation parameters when the matching is successful.

5. The method for determining a position and posture according to claim 4, characterized in that: The matching of the second scan data with the target grid map and determining the transformation parameters when the matching is successful includes: Mapping the laser points included in the second scanning data to the target grid map to obtain the position of each laser point in the target grid map; Based on the positions of the laser points contained in the second scanning data in the target grid map, determining the target point clouds corresponding to the laser points contained in the second scanning data, the target point clouds corresponding to the laser points being point clouds located within the target range of the laser points; The target point clouds corresponding to the laser points included in the second scanning data are matched with the second scanning data to determine the transformation parameters when the matching is successful.

6. The method for determining a posture according to claim 3, characterized in that: The calculating, based on the positions of the laser points contained in the first scanning data in the target grid map, the matching scores of the laser points contained in the first scanning data, comprises: If the laser point falls into the occupied area in the target grid map, the matching score of the laser point is a first value; If the laser point falls into a free area in the target grid map, the matching score of the laser point is a second value; If the laser point falls into the unknown area in the target grid map, the target obstacle point closest to the laser point is determined in the target grid map; if the distance between the laser point and the target obstacle point is less than the target distance threshold, the matching score of the laser point is the target multiple of the distance; if the distance between the laser point and the target obstacle point is greater than or equal to the target distance threshold, the matching score of the laser point is a third value; Preferably, the second value is smaller than the first value, the first value is smaller than the target multiple of the distance; the target multiple of the distance is smaller than or equal to the third value; Preferably, the sum of the matching scores of the laser points contained in the first scanning data is obtained by subtracting the target multiple of the distance and / or the third value from the sum of the first value and / or the second value; and the average value of the matching scores of the laser points contained in the first scanning data is obtained by dividing the sum of the matching scores by the number of the laser points.

7. The method for determining a position and posture according to any one of claims 1 to 6, characterized in that: The step of determining, based on the candidate particles, target particles that meet a second posture accuracy at multiple moving distances of the self-moving device includes: Based on a plurality of moving distances of the self-moving device, calculating a second posture of each of the candidate particles at each moving distance; Calculating a matching score between the second posture of each candidate particle at each moving distance and the target grid map; The candidate particles whose maximum value of the matching scores between the second posture at the multiple moving distances and the target grid map is greater than the second score threshold, and / or the candidate particles whose matching scores between the second posture at the multiple moving distances and the target grid map are greater than the third score threshold, are determined as target particles that meet the second posture accuracy.

8. A computer-readable storage medium, characterized in that: The storage medium stores a computer program, and the computer program is used to execute the posture determination method described in any one of claims 1 to 7 above.

9. An electronic device, characterized in that: include: processor; a memory for storing instructions executable by the processor; The processor is used to execute the posture determination method described in any one of claims 1 to 7 above.

10. A computer program product, characterized in that The computer program product comprises instructions, which, when executed on an electronic device, enable the electronic device to implement the posture determination method according to any one of claims 1 to 7.

Images