Repositioning method, medium, device and moving tool
Through the repositioning method of full-angle sampling and sample filtering, the positioning loss problem of mobile tools when heading angle changes large, achieving higher repositioning accuracy and rapid convergence effect.
Patent Information
- Application Number
- CN202210504134.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-05-10
- Publication Date
- 2025-09-02
- Estimated Expiration
- 2042-05-10
AI Technical Summary
When the heading angle changes greatly after the moving tool is moved or kicked, the existing repositioning technology can easily lead to positioning loss and the wrong initial positioning is taken, affecting the positioning accuracy.
Multiple angle intervals are divided into samples by a full angle average, the first sample set is generated, the sample weight value is calculated, the reasonable sample is retained, the second sample set is generated, and the sample is repositioned through the average pose of the sample, and the samples are filtered in conjunction with the map passable area to improve sampling uniformity and accuracy.
The repositioning accuracy of the moving tool after the heading angle changes after moving or kicking is improved, ensuring uniform sample distribution, and improving the convergence efficiency and positioning accuracy of the particle filtering algorithm.
Smart Images
Figure CN114894194B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of positioning technology, and in particular to a repositioning method, medium, device and moving tool. Background Art
[0002] Due to the high accuracy requirements and signal reception strength issues, smart mobile tools rely primarily on their onboard sensors to map the indoor environment and locate themselves on the map. However, during mobility, various problems such as moving, relocating, kicking, getting stuck, wheel slipping, and low steps can lead to positioning loss. Once positioning is lost, the mobile tool cannot function normally, thus affecting its intelligent performance.
[0003] Existing relocalization techniques, when initializing particle sampling, rely too heavily on the initial position, and in particular the heading, given at the time of position loss. When the actual heading differs significantly from the given approximate heading after moving or being kicked, existing sampling methods can easily produce a completely incorrect initial pose estimate, ultimately converging to the wrong pose in the wrong direction. Summary of the Invention
[0004] The purpose of the present invention is to address the defects of the prior art and provide a repositioning method, medium, device and mobile tool to improve the repositioning accuracy of the mobile tool after the heading angle changes significantly in situations such as after being moved or kicked.
[0005] To achieve the above object, the present invention provides a first aspect of a relocation method, comprising:
[0006] Obtain an estimated initial pose of the target object, and divide the entire angle into multiple angle intervals with the estimated initial pose as the center;
[0007] Sampling each of the angle intervals respectively to generate a first sample set;
[0008] Calculating a weight value for each sample in the first sample set;
[0009] Retain samples whose weight values are greater than a preset threshold, copy the retained samples, and generate a second sample set;
[0010] Calculating a weight value for each sample in the second sample set;
[0011] If the weight values of the samples in the second sample set are not all greater than the preset threshold, recopying the retained samples in the first sample set to generate a second sample set;
[0012] If the weight values of the samples in the second sample set are all greater than a preset threshold, an average pose of the samples in the second sample set is calculated, and the average pose of the samples is set as the relocalization convergent pose.
[0013] Furthermore, the method further comprises: loading and initializing a map, and obtaining a traversable area of the map;
[0014] If all samples in the first sample set are within the passable area, calculating a weight value of each sample in the first sample set;
[0015] If not all samples in the first sample set are within the passable area, sampling is performed in the angle interval corresponding to the unqualified samples, and the unqualified samples are replaced to generate the first sample set.
[0016] Furthermore, sampling each of the angle intervals to generate the first sample set specifically includes: using Gaussian sampling on each of the angle intervals to generate the first sample set.
[0017] Furthermore, the sample information in the first sample set includes two-dimensional horizontal plane coordinates and heading angles; and the samples in the first sample set obey a normal distribution.
[0018] Furthermore, the samples in the first sample set obey a normal distribution, specifically:
[0019] The two-dimensional horizontal plane coordinates of the samples in the first sample set obey the two-dimensional normal distribution of X and Y coordinates in the map coordinate system, and the heading angles of the samples in the first sample set obey the one-dimensional normal distribution of heading angles in each angle interval.
[0020] Furthermore, the number of samples taken for each of the angle intervals is the same.
[0021] Furthermore, the mean of the two-dimensional normal distribution of the X and Y coordinates is taken as the X and Y coordinates of the estimated initial pose of the target object.
[0022] Furthermore, the number of angle intervals is n, the mean of the one-dimensional normal distribution of the heading angle is (n-1 / 2)θ, and the variance is θ / 2, where θ is the angle range of each angle interval, that is, θ=360° / n.
[0023] A second aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the relocation method as described in any one of the first aspects above.
[0024] A third aspect of the present invention provides a computer device comprising a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the relocation method as described in any one of the first aspects above.
[0025] A fourth aspect of the present invention provides a mobile tool comprising the computer device described in the third aspect.
[0026] The present invention provides a repositioning method, medium, device and mobile tool, which improve the repositioning accuracy of the mobile tool after a large change in heading angle in situations such as after being moved or kicked by using multi-angle sampling in the initialization stage. BRIEF DESCRIPTION OF THE DRAWINGS
[0027] Figure 1 A first flow chart of the relocation method provided in the first embodiment of the present invention;
[0028] Figure 2 A second flow chart of the relocation method provided in the first embodiment of the present invention;
[0029] Figure 3 This is a schematic diagram of the computer device structure provided in Example 3 of the present invention. DETAILED DESCRIPTION
[0030] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments.
[0031] In an embodiment of the present invention, when the positioning of a target object is lost due to being moved or kicked, and the two-dimensional horizontal plane coordinates at the time of loss cannot be accurately obtained, multi-angle simultaneous sampling is utilized in the initialization phase, and combined with the sample validity judgment method, a particle filter is used to relocate the lost target object. Multi-angle sampling makes the sample distribution more uniform, thereby improving the convergence efficiency of the particle filter algorithm and accelerating the convergence speed.
[0032] Example 1
[0033] Figure 1 The first flow chart of the relocation method provided by the embodiment of the present invention is as follows: Figure 1 As shown, the relocation methods include:
[0034] Step 110 , obtaining an estimated initial pose of the target object, and dividing the entire angle into multiple sub-angle intervals with the estimated initial pose as the center.
[0035] Specifically, the estimated initial pose of the target object uses a map coordinate system, also called a global coordinate system. This coordinate system is defined with the position of the target object when it is powered on as the coordinate origin, the target object's orientation as the X-axis, the vertical direction of the target object's orientation as the Y-axis, and the height direction as the Z-axis, following the right-hand rule.
[0036] The estimated initial position of the target object is obtained as the estimated initial position of the target object in the map coordinate system, which may include X, Y, Z axis coordinates and heading angle information.
[0037] In some possible implementations, since the movement of the target object on a two-dimensional horizontal plane is mainly considered, the Z-axis coordinate may be ignored, and only the X and Y two-dimensional horizontal plane coordinates of the estimated initial pose of the target object need to be obtained.
[0038] In some possible implementations, since sampling is performed through multi-angle sampling, the heading angle when positioning is lost may not be considered.
[0039] In one embodiment, assuming that the estimated initial pose coordinates of the target object after it loses position are A(x0, y0), the 360-degree area centered on A is evenly divided into n angular intervals, with each angular interval having an angular range of θ = 360° / n. For example, if n = 18, the 360-degree area centered on A is divided into 18 angular intervals, with each angular interval having an angular range of θ = 20°.
[0040] Step 120: Sample each angle interval to generate a first sample set.
[0041] Specifically, when positioning is lost due to movement or kicking, the heading angle may have changed significantly, deviating significantly from the possible heading angle obtained at the time of loss. To ensure the validity of the collected samples, the heading angle is sampled over a wide range of 360 degrees. Samples are collected in each angle interval, and the samples collected in all intervals form the first sample set.
[0042] In some possible implementations, step 120 performs sampling according to the Gaussian sampling method, and the samples in the obtained first sample set obey a normal distribution.
[0043] Specifically, the sample information in the first sample set includes two-dimensional horizontal plane coordinates and heading angles. The two-dimensional horizontal plane coordinates of the samples in the first sample set follow a two-dimensional normal distribution of X and Y coordinates in the map coordinate system, and the heading angles of the samples in the first sample set follow a one-dimensional normal distribution of heading angles in each angle interval. Preferably, the mean of the two-dimensional normal distribution of X and Y coordinates is the X and Y coordinates of the estimated initial pose of the target object.
[0044] In another preferred solution, the number of samples sampled for each angle interval is the same, thereby ensuring that the sampling of each angle interval in the full angle range is relatively fair, and reducing the error rate after the filtering result converges.
[0045] In a preferred specific example, it is assumed that the estimated initial pose coordinates obtained after the target object loses its position are A(x0, y0), where the coordinates of the estimated initial pose can be the last set of position information obtained by the sensor when the target object loses its position. The number of angle intervals n=18, then the degree of each angle interval θ=360° / 18=20°, the first angle interval is 0°~20°, the second angle interval is 20°~40°, the third angle interval is 40°~60°, ..., and so on. The nth angle interval is (n-1)θ~nθ (1≤n≤18). Preferably, the number of samples in each angle interval is consistent and is set to M. In order to cover the largest possible horizontal plane area, M=200 is taken in this embodiment. When the number of sampling samples and the sampling method of each angle interval are the same, the sampling of one of the angle intervals is described in detail below.
[0046] Select the angle interval (n-1)θ~nθ for normal distribution sampling, and let the sample be Bn(x i ,y i ), 1≤i≤200, then the first sample set can be represented by a vector {B1, B2, ..., Bn}, where B1 to Bn contain a total of nM samples. When n = 18 and M = 200, a total of 18x200 = 3600 samples are contained. The sample mean μ of the normal distribution function of the horizontal position is n1 To estimate the coordinates of the initial pose A(x0,y0), that is, r = 0, the variance S n1 is r = 1.5 meters, where r is the distance from the point on the horizontal plane to the estimated initial pose coordinate A(x0, y0). The sample mean μ of the normal distribution function of the sampling angle n2 The angle of the distribution is (n-1 / 2)θ, the variance is θ / 2, and 200 samples are randomly generated in the angle range of (n-1)θ to nθ based on the normal distribution function defined above.
[0047] Preferably, the plane coordinates and heading angles of the 200 random samples are generated within the range of 2 times the standard deviation. The standard deviation is the square root of the variance. According to the characteristics of the normal distribution, the probability that the sample is within the range of 2 times the standard deviation around the mean is 95.4%. Therefore, for the sampling of the sample heading angle, the samples are mainly concentrated in Within the range, when the number of angle intervals is 18, θ=20°, then That is, within the 20° angle range, the normal distribution random sampling is concentrated in the 6.32°x2=12.64° angle range. Therefore, compared with the 20° angle range, the angle range of the sampled heading angle is small, and the consistency of the heading angle is high. The sampling results of each angle interval can be distinguished according to the heading angle of the sample. From the perspective of the 360° angle range, the distribution of the heading angles of all samples is more uniform, and the correct angle value will not be missed. On the horizontal plane, 95.4% of the generated samples are concentrated in the (n-1)θ~nθ angle range. Therefore, in this specific example, the 200 samples randomly sampled from the angle range (n-1)θ to nθ are mostly located within the area of r = 2.44 meters, and the heading angles are mostly within Within the interval, when n=1, the heading angle is within the interval of 10±6.32°; when n=2, the heading angle is within the interval of 30±6.32°; and so on. It can be seen that the heading angles in each interval are completely non-overlapping, and the coordinates of the two-dimensional horizontal plane of the samples in each interval are also non-coincident.
[0048] Before step 110, Figure 2 As shown, the relocation method further includes:
[0049] Step 210: load and initialize a map, and obtain the traversable area of the map.
[0050] Specifically, the global map required by the system is loaded, preferably using a gridded map. According to the grid map protocol, the state represented by each grid cell in the grid map is calculated. For example, cells with obstacles or unknown areas are marked as 1, and cells in traversable areas are marked as 0. Traversable areas are areas where the LiDAR detects no obstacles, while unknown areas are areas that the LiDAR cannot detect.
[0051] After step 120, if Figure 2 As shown, the relocation method further includes:
[0052] Step 310: If not all samples in the first sample set are within the passable area, sampling is performed in the angle interval corresponding to the unqualified samples, the unqualified samples are replaced, and the first sample set is generated again.
[0053] If all samples in the first sample set are within the passable area, step 130 is executed.
[0054] Specifically, the coordinates of all samples in the first sample set are compared to see if they all fall within the grid marked 0. If not all fall within the grid marked 0, and there are H samples that fall within the grid marked 1, then these H samples are judged to be invalid samples. Based on the heading angles of these H samples, the angle intervals to which these H samples belong are determined. These H invalid samples are deleted, and resampled in the angle intervals to which these H invalid samples belong, padding the samples in the first sample set to the preset value. If the sampled sample is in an unknown area or an obstacle area, it is actually a meaningless sample. It is removed, padded, and the sample validity is repeatedly judged until all collected samples are in a passable area, thus ensuring that all samples are meaningful. This results in higher accuracy and faster convergence in the subsequent sample processing and calculation.
[0055] In a specific example, when the angle interval n=18 and each angle interval takes the same sample value M=200, the preset value of the samples in the first sample set is 200 samples per angle interval, and a total of 18x200=3600 samples need to be collected. Assuming that the number of H invalid samples is 300, there are 50 invalid samples in the interval n=1, 130 invalid samples in the interval n=3, and 120 invalid samples in the interval n=4. Then, the intervals n=1, n=3, and n=4 need to be padded with 50, 130, and 120 samples, respectively. The method for generating the samples to be padded is the same as that in step 120 and will not be repeated here.
[0056] Step 130: Calculate the weight value of each sample in the first sample set.
[0057] Specifically, the weight values are calculated in turn according to the kinematic model and the observation model. In a preferred specific example, the kinematic model adopts the odometry model, and the observation model adopts the likelihood field model. When the target object loses its positioning and travels a certain distance again, the heading angle of the target object at the starting position and the end position of this distance, as well as the distance between the two positions, will appear. Through the calculation of the odometry model, each sample can predict the new distribution at the end position from the distribution at the starting position, and generate a new posture at the end position. Under each new posture, the Euclidean distance between the laser scanning point mapped to the grid map of the target object and the corresponding nearest obstacle is calculated. The sum of the Euclidean distances calculated after one circle of laser scanning for each sample is compared and recorded as T. ni , where T ni The sum of the Euclidean distances of the i-th sample in the n-th angle interval is represented. The smaller the sum, the closer the current posture of the sample is to the correct posture. Conversely, the further it deviates from the correct posture. Calculate the weight of each sample in the first sample set. Preferably, let W ni =1 / Tni , then the weight value Q of the i-th sample in the n-th angle interval is ni W ni With all W ni The ratio of and.
[0058] Step 140: retain samples whose weight values are greater than a preset threshold, copy the retained samples, and generate a second sample set.
[0059] Specifically, after calculating the weight of each sample in the first sample set, each sample is compared against a preset first threshold. The first threshold is used to determine whether the sample is close to the correct pose and is retained. Samples with weights greater than the first threshold are retained, and a preset number of samples are randomly generated around the retained samples. The generated preset number of samples and the retained samples with weights greater than the first threshold are combined to form a second sample set.
[0060] Step 150, calculating the weight value of each sample in the second sample set;
[0061] Specifically, the method for calculating the weight value of each sample in the second sample set is the same as the calculation method in step 106 , and will not be described in detail here.
[0062] Step 160 : If the weight values of the samples in the second sample set are not all greater than the preset threshold, the retained samples in the first sample set are re-copied to generate the second sample set.
[0063] Specifically, when the weight value of the sample in the second sample set is less than the first threshold, it means that the posture of some samples is incorrect. It is necessary to re-copy the retained samples with close to correct postures and judge the weight value. After multiple operations, until the sample distribution in the second sample set converges.
[0064] Step 170 : If the weight values of the samples in the second sample set are all greater than the preset threshold, the average pose of the samples in the second sample set is calculated, and the average pose of the samples is set as the relocalization convergence pose.
[0065] Specifically, the X, Y and heading angles of the samples in the final converged second sample set are averaged to obtain the average pose of the second sample set. The average pose is the final relocalization convergence pose, and the relocalization is completed.
[0066] Example 2
[0067] A second embodiment of the present invention provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, any one of the relocation methods provided in the first embodiment is implemented.
[0068] Example 3
[0069] A third embodiment of the present invention provides a computer device, Figure 3 This is a schematic diagram of the computer device structure provided in the third embodiment of the present invention. Figure 3 As shown, it includes a memory 100, a processor 200 and a computer program stored in the memory, and the processor executes the computer program to implement any one of the relocation methods provided in the above embodiment 1.
[0070] Example 4
[0071] An embodiment of the present invention provides a mobile tool, including the computer device of the third embodiment.
[0072] The mobile tool referred to in this application may be a vehicle device or a robotic device having the following functions:
[0073] (1) Passenger-carrying function, such as family cars and buses;
[0074] (2) Cargo carrying function, such as ordinary trucks, box trucks, trailer trucks, closed trucks, tank trucks, flatbed trucks, container trucks, dump trucks, trucks with special structures, etc.;
[0075] (3) Tool functions, such as logistics delivery vehicles, automated guided vehicles (AGVs), patrol cars, cranes, hoists, excavators, bulldozers, forklifts, rollers, loaders, off-road engineering vehicles, armored engineering vehicles, sewage treatment vehicles, sanitation vehicles, vacuum trucks, floor scrubbers, sprinkler trucks, sweeping robots, food delivery robots, shopping guide robots, lawn mowers, golf carts, etc.;
[0076] (4) Entertainment functions, such as entertainment vehicles, amusement park self-driving devices, balance vehicles, etc.;
[0077] (5) Special rescue functions, such as fire trucks, ambulances, power repair trucks, engineering rescue trucks, etc.
[0078] Professionals should also be further aware that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the above description has generally described the components and steps of each example according to their functions. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professionals and technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.
[0079] The steps of the methods or algorithms described in conjunction with the embodiments disclosed herein may be implemented using hardware, a software module executed by a processor, or a combination of the two. The software module may be placed in a random access memory (RAM), a memory, a read-only memory (ROM), an electrically programmable ROM, an electrically erasable programmable ROM, a register, a hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.
[0080] The specific implementation methods described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation method of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.
Claims
1. A relocation method, characterized in that: The method comprises: Obtain an estimated initial pose of the target object, and divide the entire angle into multiple angle intervals with the estimated initial pose as the center; Sampling each of the angle intervals respectively to generate a first sample set; Calculating a weight value for each sample in the first sample set; Retain samples whose weight values are greater than a preset threshold, copy the retained samples, and generate a second sample set; Calculating a weight value for each sample in the second sample set; If the weight values of the samples in the second sample set are not all greater than the preset threshold, recopying the retained samples in the first sample set to generate a second sample set; If the weight values of the samples in the second sample set are all greater than a preset threshold, calculating the average pose of the samples in the second sample set, and setting the average pose of the samples as the relocalization convergent pose; The sample information in the first sample set includes heading angles, and the heading angles obey a one-dimensional normal distribution of heading angles in each angle interval; The number of angle intervals is n, the mean of the one-dimensional normal distribution of the heading angle is (n-1 / 2)θ, and the variance is θ / 2, where θ is the angle range of each angle interval, that is, θ=360° / n.
2. The relocation method according to claim 1, wherein: The method further comprises: Loading and initializing a map, and obtaining a traversable area of the map; If all samples in the first sample set are within the passable area, calculating a weight value of each sample in the first sample set; If not all samples in the first sample set are within the passable area, sampling is performed in the angle interval corresponding to the unqualified samples, and the unqualified samples are replaced to generate the first sample set; Among them, unqualified samples are those located in inaccessible areas.
3. The relocation method according to claim 1, wherein: Sampling each of the angle intervals to generate a first sample set is specifically as follows: Gaussian sampling is performed on each of the angle intervals to generate a first sample set.
4. The relocation method according to claim 3, wherein: The sample information in the first sample set includes two-dimensional horizontal plane coordinates and heading angles; and the samples in the first sample set obey a normal distribution.
5. The relocation method according to claim 4, wherein: The samples in the first sample set obey the normal distribution, specifically: The two-dimensional horizontal plane coordinates of the samples in the first sample set obey the two-dimensional normal distribution of X and Y coordinates in the map coordinate system, and the heading angles of the samples in the first sample set obey the one-dimensional normal distribution of heading angles in each angle interval.
6. The relocation method according to claim 1, wherein: The number of samples taken for each angle interval is the same.
7. The relocation method according to claim 5, wherein: The mean of the two-dimensional normal distribution of the X and Y coordinates is the X and Y coordinates of the estimated initial pose of the target object.
8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.
9. A computer device comprising a memory, a processor, and a computer program stored in the memory, wherein: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 7.
10. A mobile tool, characterized in that: Comprising the computer device as claimed in claim 9.