A method for mobile robot positioning recovery based on Monte Carlo positioning method

Abstract

The present invention discloses a method for mobile robot positioning recovery based on the Monte Carlo positioning method. The method comprises calculating a particle set, short-term measurement likelihood, long-term measurement likelihood, and average confidence at the current moment to determine whether a kidnapping event has occurred on the mobile robot; calculating a Student's distribution of recovery auxiliary particles based on the positioning failure duration; determining whether to add recovery auxiliary particles to the particle set based on a long-short time strategy and adjusting the ratio of recovery auxiliary particles to ordinary particles in the particle set; adding recovery auxiliary particles to the particle set; sampling the recovery auxiliary particles in the particle set based on the Student's distribution based on the positioning failure duration; sampling ordinary particles in the particle set based on order importance; and combining the sampled recovery auxiliary particles and ordinary particles to form a new particle set. After a kidnapping event occurs, the recovery auxiliary particles added to the particle set by the present invention can ensure that particles are distributed at the mobile robot's true position.

Description

Technical Field

[0001] The present invention relates to the technical field of robot positioning control, in particular to a mobile robot positioning recovery method based on a Monte Carlo positioning method. Background Art

[0002] With the improvement of the degree of automation in the manufacturing industry, more and more autonomous mobile robots are used in production workshops, which puts higher requirements on the robustness of the positioning system of mobile robots.

[0003] The kidnapping problem (KRP) is one of the important issues that need to be studied in mobile robot positioning systems. The kidnapping problem (or kidnapping event) specifically refers to the phenomenon in which a mobile robot is instantaneously moved to a location for which there is no prior information during normal operation, resulting in positioning failure. In the dynamic environment of a production workshop, large crowds, the handling of large items, and poor sensor contact can cause the mobile robot's positioning system to lose data, leading to kidnapping events. The recovery success rate and speed of the mobile robot's positioning system after a kidnapping event reflect the reliability of the positioning system.

[0004] In the field of probabilistic localization, the Monte Carlo Localization (MCL) method specifically uses the particle with the largest weight to represent the position of the mobile robot. It is more commonly used in the initial pose estimation of the mobile robot, but can also be used for the positioning recovery of the mobile robot.

[0005] Furthermore, during operation, the Augmented MCL (A-MCL) method can add evenly distributed recovery auxiliary particles to the map. The added recovery auxiliary particles are adaptively increased based on the reliability of long-term and short-term observations. Using the Augmented MCL method can avoid the problem of no particle distribution in the actual position of the mobile robot, thereby improving the performance of the Monte Carlo localization method.

[0006] However, as the failure time of positioning the mobile robot increases, after several iterations of the enhanced Monte Carlo positioning method, the particles will quickly spread to the entire map environment, making it difficult to ensure that the particles are distributed at the actual position of the mobile robot; in addition, when the mobile robot is subject to extreme interference and cannot recover its positioning in a short time, after several iterations of the enhanced Monte Carlo positioning method, the particles will also quickly spread to the entire map environment, making it difficult to ensure that the particles are distributed at the actual position of the mobile robot; both of the above situations make it impossible for the mobile robot to recover its positioning in a short time, making it difficult to adapt to the operating efficiency of the production workshop. Summary of the Invention

[0007] The object of the present invention is to provide a mobile robot positioning recovery method based on the Monte Carlo positioning method, which can quickly restore the positioning of the mobile robot after a kidnapping incident occurs.

[0008] To achieve the above object, the present invention provides the following technical solution: a method for recovering the positioning of a mobile robot based on the Monte Carlo positioning method, comprising the following steps:

[0009] S1, based on: the sensor measurement data Z of the mobile robot at the current time t t and odometer measurement data u t , and the particle set L at the previous moment t-1 t-1 , short-term measurement likelihood ψ s t-1 and the long-term measurement likelihood ψ l t-1 ;

[0010] Calculation results: particle set L at the current time t t , short-term measurement likelihood ψ s , long-term measurement likelihood ψ l and the average confidence ψ mean ;

[0011] Furthermore, it is determined whether a kidnapping incident occurs with the mobile robot; if so, the process proceeds to step S2; if not, the process skips steps S2 to S4 and proceeds to step S5;

[0012] S2. Calculate and obtain the particle set L t Student's distribution of the recovery auxiliary particles based on the positioning failure time

[0013] S3, judging whether to send the particle set L to the particle set L according to the long and short time strategy t Add recovery auxiliary particles in the particle set L, and further determine that the recovery auxiliary particles are in the particle set L t to adjust the particle set L t The ratio of recovery auxiliary particles to ordinary particles in;

[0014] S4, calculate and determine the particle set L t The number of recovery auxiliary particles M added in sd , and to the particle set L t Add recovery auxiliary particles;

[0015] S5. The Student's distribution based on the positioning failure duration obtained in step S2 is For the particle set L t The recovered auxiliary particles in the sample are sampled;

[0016] S6. According to the order importance, the particle set L t Sampling of ordinary particles in

[0017] S7, combining the recovery auxiliary particles obtained by sampling and the ordinary particles to form a new particle set L t , and based on the particle set L t Steps S1 to S6 are performed in a loop to maintain the positioning of the mobile robot.

[0018] In the above technical solution, at any time t, the particle set is:

[0019]

[0020] in, is the particle set L t The position of the nth particle in at time t, is the particle set L t The weight of the nth particle in the particle set L, k is t The number of particles in

[0021] For the previous moment t-1, the particle set is:

[0022]

[0023] in, is the particle set L t-1 The position of the nth particle in at the last moment t-1, is the particle set L t-1 The weight of the nth particle in the particle set L, k is t-1 The number of particles in .

[0024] In the above technical solution, in step S1, the particle set L at the current time t t , short-term measurement likelihood ψ s , long-term measurement likelihood ψ l and the average confidence ψ mean The specific calculation methods include:

[0025] S1.1. Express the confidence of the position of the mobile robot at any time t as:

[0026] bel(l t )=P(l t |z t ,u t ,m);

[0027] Among them, l t is the position of the mobile robot at the current time t, and lt ~(x t ,y t ,θ t ), Z t is the sensor measurement data of the mobile robot at the current time t, u t is the odometer measurement data of the mobile robot at the current time t, and m is the prior grid map data;

[0028] S1.2, through the motion model P(l t |u t ,l t-1 ), predict the position l of the mobile robot at the current time t t , and the pose prediction confidence of the mobile robot at the current time t is expressed as:

[0029]

[0030] Among them, l t-1 is the position of the mobile robot at the previous moment t-1;

[0031] S1.3, combining the sensor measurement data Z of the mobile robot at the current time t t , the pose confidence of the mobile robot at the current time t is expressed as:

[0032]

[0033] Among them, η is the normalization factor, P(z t |l t ,m) is the posture measurement model of the mobile robot;

[0034] S1.4. Constructing a particle set by a set of weighted particles The particle set L t The confidence level is expressed as:

[0035]

[0036] in, is the particle set L t The position of the nth particle in at time t, is the particle set L t The weight of the nth particle in L, δ is the Dirac-delta function, k is the particle set L t The number of particles in

[0037] S1.4, according to the particle set at the previous moment t-1 The motion model P(l t |ut ,l t-1 ) Sampling to obtain the particle set L at the current time t t ;

[0038] S1.5, through the posture measurement model P(z t |l t ,m) calculate and obtain the particle set L t The weight of each particle in And the weight Normalize and calculate the particle set L t The average confidence of each particle in mean ;

[0039] S1.6. Calculate the short-term measurement likelihood ψ using the following formula: s and the long-term measurement likelihood ψ l :

[0040] ψ s =ψ s +τ short (ψ mean -ψ s );

[0041] ψ l =ψ l +τ long (ψ mean -ψ l );

[0042] Among them, τ short is the average decay rate of the Monte Carlo filter in a short period of time, τ long is the average decay rate of the Monte Carlo filter over a long period of time.

[0043] In the above technical solution, in step S1, the method for determining whether a kidnapping incident has occurred on the mobile robot is specifically as follows:

[0044] S1a, using information entropy Entropy (t) to represent the particle set L t The degree of particle dispersion in , the information entropy Entropy (t) is specifically:

[0045]

[0046] S2a. Preset a threshold value β of the information entropy. If the information entropy Entropy(t) satisfies Entropy(t)>β at any time t, it is determined that a kidnapping incident occurs to the mobile robot at the time t; if the information entropy Entropy(t) fails to satisfy Entropy(t)<β, it is determined that no kidnapping incident occurs to the mobile robot.

[0047] In the above technical solution, in step S2, the Student's distribution of the restoration auxiliary particles based on the positioning failure time is Specifically:

[0048]

[0049] Wherein, μ is the position of the mobile robot when the positioning data fails, n is the particle set L t The number of particles at the current moment, t kpd is the time elapsed by the mobile robot from the moment the kidnapping incident occurred to the current moment, v is the running speed of the mobile robot, Γ is the gamma function, h is the extreme distance condition of the mobile robot's movement, specifically h = n·S, and S is the circumference of the environment map.

[0050] In the above technical solution, step S3 is specifically as follows:

[0051] By max(0.0,1-ψ s / ψ l ) value to determine whether to add the particle set L t Add recovery auxiliary particles;

[0052] And through 1-ψ s / ψ l Calculate the particle set L t The ratio of recovery auxiliary particles to normal particles in .

[0053] In the above technical solution, by max(0.0,1-ψ s / ψ l ) value to determine whether to add the particle set L t Add recovery auxiliary particles, including:

[0054] If ψ s ≥ψ l , then max(0.0,1-ψ s / ψ l )=0, no need to add the particle set L t Add recovery auxiliary particles;

[0055] If ψ s <ψ l , then max(0.0,1-ψ s / ψ l )=1, it is necessary to add the particle set L t Add recovery auxiliary particles.

[0056] In the above technical solution, in step S4, the particle set L tThe number of recovery auxiliary particles added is M sd , and specifically:

[0057]

[0058] Where F is a circle with the kidnapping incident as the center and a radius of 2v·t kpd The circular area, and t kpd is the time the mobile robot has experienced from the moment the kidnapping incident occurred to the current moment, v is the running speed of the mobile robot, and D is the particle set L t The particle density in It is an empirical function of efficiency loss. When F / D is too large, F / D is appropriately reduced to ensure sampling efficiency.

[0059] In the above technical solution, in step S5, if the restoration auxiliary particles are added in the obstacle area or unknown area of the map, the restoration auxiliary particles are resampled.

[0060] In the above technical solution, in step S5, when resampling the recovery auxiliary particles, the specific rules are as follows:

[0061] With the kidnapping incident as the center, establish a circle with a radius of 2v·t kpd If the unknown area occupies 3 / 4 or more of the circular judgment area, the position is evenly distributed. Take samples;

[0062] If the area of the circular judgment area is greater than π, then the circular judgment area is uniformly distributed along the direction Sample the region U(-π,π).

[0063] Compared with the prior art, the present invention has the following beneficial effects:

[0064] 1. The mobile robot positioning recovery method based on the Monte Carlo positioning method of the present invention is to use the particle set L t The weight of each particle in the particle set L can determine the position information of the mobile robot. After the kidnapping incident occurs, t The recovery auxiliary particles added in the ,can ensure that particles are distributed at the real position of the mobile robot;,and, through the Student's distribution based on the positioning failure, To constrain the added recovery auxiliary particles, thereby setting an effective search range, it is possible to improve the recovery success rate and recovery speed of the mobile robot positioning after the kidnapping incident; in addition, through the long-short time strategy, the recovery auxiliary particles and ordinary particles in the particle set L are adjusted. t The ratio in can maintain the short-term measurement likelihood ψ s, and in determining the number of recovered auxiliary particles together with the long-term measurement likelihood ψ l associated.

[0065] 2. The mobile robot positioning recovery method based on the Monte Carlo positioning method of the present invention is to send the particle set L to the particle set L according to the duration of the kidnapping incident. t The time-varying recovery auxiliary particles are added to increase the possibility that the recovery auxiliary particles appear near the real pose of the mobile robot, thereby further improving the recovery success rate and recovery speed of the mobile robot positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 It is a flow chart of the steps of the present invention.

[0067] Figure 2 Schematic diagram of the recovery-assisting particle distribution after a kidnapping incident occurs on a mobile robot according to the present invention. DETAILED DESCRIPTION

[0068] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0069] Example 1:

[0070] This embodiment provides a method for recovering the positioning of a mobile robot based on the Monte Carlo localization method. The method is based on sensor measurement data and odometer measurement data of the mobile robot and uses the Monte Carlo localization method (MCL). The method can quickly recover the positioning of the mobile robot after a kidnapping incident occurs. The positioning includes position information and posture information of the mobile robot (position information and posture information may also be collectively referred to as posture information).

[0071] First, the Monte Carlo localization method requires releasing a number of particles in a map (the operating space of the mobile robot), and the set of particles is called a particle set.

[0072] See also Figure 1 The mobile robot positioning recovery method based on the Monte Carlo positioning method of this embodiment includes the following steps:

[0073] S1, based on: the sensor measurement data Z of the mobile robot at the current time t t and odometer measurement data u t , and the particle set L at the previous moment t-1 t-1 , short-term measurement likelihood ψs t-1 and the long-term measurement likelihood ψ l t-1 ;

[0074] Calculation results: particle set L at the current time t t , short-term measurement likelihood ψ s , long-term measurement likelihood ψ l and the average confidence ψ mean ;

[0075] Furthermore, it is determined whether a kidnapping incident has occurred with the mobile robot; if so, the process proceeds to step S2; if not, the process skips steps S2 to S4 and proceeds to step S5;

[0076] S2. Calculate and obtain particle set L t Student's distribution of the recovery auxiliary particles based on the positioning failure time

[0077] S3, according to the long and short time strategy, determine whether to move to the particle set L t Add recovery auxiliary particles and further determine the recovery auxiliary particles in the particle set L t to adjust the particle set L t The ratio of recovery auxiliary particles to ordinary particles in;

[0078] S4, calculate and determine the particle set L t The number of recovery auxiliary particles M added in sd , and to the particle set L t Add recovery auxiliary particles;

[0079] S5. The Student's distribution based on the positioning failure duration obtained in step S2 is For particle set L t The recovered auxiliary particles in the sample are sampled;

[0080] S6. According to the order importance, the particle set L t Sampling of ordinary particles in

[0081] S7, combining the sampled recovery auxiliary particles and ordinary particles to form a new particle set L t , and based on the particle set L t Steps S1 to S6 are repeated in a loop to maintain the positioning of the mobile robot.

[0082] The mobile robot positioning recovery method based on the Monte Carlo positioning method of this embodiment uses the particle set L t The weight of each particle in the particle set L can determine the position information of the mobile robot. After the kidnapping incident occurs, tThe recovery auxiliary particles added in the ,can ensure that there are particles distributed at the real pose of the mobile robot, and,through the Student's distribution based on the positioning failure time, To constrain the added recovery auxiliary particles, thereby setting an effective search range, it is possible to improve the recovery success rate and recovery speed of the mobile robot positioning after a kidnapping incident.

[0083] Specifically, for any time t, the particle set is:

[0084]

[0085] in, is the particle set L t The position of the nth particle in at time t, is the particle set L t The weight of the nth particle in the set L, k is the weight of the particle set L t The number of particles in .

[0086] The particle set contains the particle's posture information and weight information. The particle's posture information refers to the posture that the mobile robot may reach, and the particle's weight information refers to the probability of the mobile robot reaching a certain posture.

[0087] Correspondingly, for the previous moment t-1, the particle set is:

[0088]

[0089] in, is the particle set L t-1 The position of the nth particle in at the last moment t-1, is the particle set L t-1 The weight of the nth particle in the set L, k is the weight of the particle set L t-1 The number of particles in .

[0090] Specifically, in step S1, the particle set L at the current time t t , short-term measurement likelihood ψ s , long-term measurement likelihood ψ l and the average confidence ψ mean The specific calculation methods include:

[0091] S1.1. Express the confidence of the mobile robot's posture at any time t as:

[0092] bel(l t )=P(l t |z t ,u t ,m);

[0093] Among them, lt is the position of the mobile robot at the current time t, and l t ~(x t ,y t ,θ t ), Z t is the sensor measurement data of the mobile robot at the current time t, u t is the odometer measurement data of the mobile robot at the current time t, and m is the prior grid map data;

[0094] S1.2, through the motion model P(l t |u t ,l t-1 ), predict the position l of the mobile robot at the current time t t , and the predicted confidence of the mobile robot's posture at the current time t is expressed as:

[0095]

[0096] Among them, l t-1 is the position of the mobile robot at the previous moment t-1;

[0097] S1.3, combined with the sensor measurement data Z of the mobile robot at the current time t t , the pose confidence of the mobile robot at the current time t is expressed as:

[0098]

[0099] Among them, η is the normalization factor, P(z t |l t ,m) is the pose measurement model of the mobile robot;

[0100] S1.4. Constructing a particle set by a set of weighted particles The particle set L t The confidence level is expressed as:

[0101]

[0102] in, is the particle set L t The position of the nth particle in at time t, is the particle set L t The weight of the nth particle in, δ is the Dirac-delta function, k is the particle set L t The number of particles in

[0103] S1.4, according to the particle set at the previous moment t-1 By moving the robot's motion model P(l t|u t ,l t-1 ) Sampling to obtain the particle set L at the current time t t ;

[0104] S1.5, by moving the robot's posture measurement model P(z t |l t ,m) calculate and obtain the particle set L t The weight of each particle in And the weight Normalize and calculate the particle set L t The average confidence of each particle in mean ;

[0105] S1.6. Calculate the short-term measurement likelihood ψ using the following formula: s and the long-term measurement likelihood ψ l :

[0106] ψ s =ψ s +τ short (ψ mean -ψ s );

[0107] ψ l =ψ l +τ long (ψ mean -ψ l );

[0108] Among them, τ short is the average decay rate of the Monte Carlo filter in a short period of time, τ long is the average decay rate of the Monte Carlo filter over a long period of time. In general, 0≤τ long <<τ short .

[0109] In this step, the short-term measurement likelihood ψ is pre-calculated s , long-term measurement likelihood ψ l and the average confidence ψ mean , which can facilitate the subsequent judgment of kidnapping incidents and long-term and short-term strategy judgments.

[0110] Specifically, considering that the success rate of the mobile robot's positioning is calculated by the Monte Carlo positioning method, it is reflected in the particle set L t The degree of dispersion of the particle set L is better expressed by information entropy. t Therefore, this embodiment uses the particle set L tThe information entropy contained in the mobile robot is used to determine whether a kidnapping incident occurs to the mobile robot; more specifically, in step S1, the method for determining whether a kidnapping incident occurs to the mobile robot is as follows:

[0111] S1a, use information entropy Entropy(t) to represent the particle set L t The degree of particle dispersion in , information entropy (t) is specifically:

[0112]

[0113] S2a. A threshold value β of information entropy is preset. If the information entropy Entropy(t) satisfies Entropy(t)>β at any time t, it is determined that a kidnapping incident has occurred on the mobile robot at that time t; if the information entropy Entropy(t) does not satisfy Entropy(t)>β, it is determined that no kidnapping incident has occurred on the mobile robot.

[0114] Specifically, in step S2, the Student's distribution of the auxiliary particles based on the positioning failure time is restored. Specifically:

[0115]

[0116] Among them, μ is the position of the mobile robot when the positioning data fails, n is the particle set L t The number of particles at the current moment, t kpd is the time elapsed by the mobile robot from the moment the kidnapping incident occurred to the current moment, v is the running speed of the mobile robot, Γ is the gamma function, h is the extreme distance of the mobile robot's movement, specifically h = n·S, and S is the circumference of the environment map.

[0117] After a mobile robot is kidnapped, it is necessary to restore its positioning by adding recovery auxiliary particles. In a real-world environment, factors that affect the mobile robot's search include the size of the mobile robot's operating space, the mobile robot's operating speed, and the time elapsed from the moment the kidnapping occurred to the current moment. The longer the kidnapping event lasts, the larger the search space needs to be. In summary, uniform sampling of all recovery auxiliary particles is obviously not reasonable. Therefore, sampling recovery auxiliary particles using a Student's distribution based on the duration of positioning failure can improve the success rate and speed of mobile robot positioning recovery.

[0118] Specifically, step S3 is as follows:

[0119] By max(0.0,1-ψ s / ψ l ) value to determine whether to move to the particle set Lt Add recovery auxiliary particles;

[0120] And through 1-ψ s / ψ l Calculate the particle set L t The ratio of recovery auxiliary particles to normal particles in .

[0121] More specifically, by max(0.0,1-ψ s / ψ l ) value to determine whether to move to the particle set L t Add recovery auxiliary particles, including:

[0122] If ψ s ≥ψ l , then max(0.0,1-ψ s / ψ l )=0, no need to add particle set L t Add recovery auxiliary particles;

[0123] If ψ s <ψ l , then max(0.0,1-ψ s / ψ l )=1-ψ s / ψ l , we need to add the particle set L t Add recovery auxiliary particles.

[0124] From the above formula, we can see that step S3 actually adjusts the recovery auxiliary particles and ordinary particles in the particle set L. t The proportion of the particle set L is constant, while the total number of particles (the sum of the recovery auxiliary particles and the ordinary particles) remains unchanged; in step S4, due to the addition of the recovery auxiliary particles, the particle set L t The total number of particles varies.

[0125] In the Monte Carlo positioning method, adding a fixed number of recovery auxiliary particles lacks effectiveness and universality; in determining the number of recovery auxiliary particles and ordinary particles in the particle set L t In order to improve the effectiveness and versatility of the algorithm, considering that when adding recovery auxiliary particles, the accuracy of the added recovery auxiliary particles can be improved by relying on the confidence of the observed sensor, the long-short time filter is used to determine the ratio of the added recovery auxiliary particles to the ordinary particles, which can maintain the short-term measurement likelihood ψ s , and in determining the number of recovered auxiliary particles together with the long-term measurement likelihood ψ l associated.

[0126] Example 2:

[0127] This embodiment provides a method for recovering the positioning of a mobile robot based on the Monte Carlo positioning method. Based on the method for recovering the positioning of a mobile robot based on the Monte Carlo positioning method described in the first embodiment, the method further includes the following technical solutions:

[0128] To ensure the effectiveness of positioning, recovery auxiliary particles can only be generated in the obstacle-free area (white part) of the map. If the recovery auxiliary particles appear in the obstacle area (black part) or unknown area (gray part) of the map, the recovery auxiliary particles need to be resampled.

[0129] Therefore, this embodiment further includes: in step S5, if the restoration auxiliary particles are added to the obstacle area or unknown area of the map, the restoration auxiliary particles are resampled.

[0130] After the abduction event, the distribution of the auxiliary particles is restored as Figure 2 As shown; Figure 2 As shown in a, in the initial stage after the kidnapping event, it is more inclined to sample the recovery auxiliary particles near the kidnapping event point, because the mobile robot at this time will not be too far away from the kidnapping event point; as time goes by, as Figure 2 As shown in b, the probability of the recovery auxiliary particles being generated at a location far away from the point where the kidnapping event occurs will gradually increase; when a long time has passed since the kidnapping event, in order to avoid excessive repeated sampling of the recovery auxiliary particles, it is necessary to Figure 2 The space shown in c is uniformly sampled.

[0131] Therefore, this embodiment further includes:

[0132] In step S5, when resampling the recovery auxiliary particles, the specific rules are as follows:

[0133] With the kidnapping incident as the center, establish a circle with a radius of 2v·t kpd If the unknown area occupies 3 / 4 or more of the circular judgment area, the position is evenly distributed. Take samples;

[0134] If the area of the circular judgment area is greater than π, then the Sample the region U(-π,π).

[0135] Example 3:

[0136] This embodiment provides a method for recovering the positioning of a mobile robot based on the Monte Carlo positioning method. Based on the method for recovering the positioning of a mobile robot based on the Monte Carlo positioning method described in Example 1 or Example 2, the method further includes the following technical solutions:

[0137] In step S4, the particle set L t The number of recovery auxiliary particles added is M sd , and specifically:

[0138]

[0139] Where F is a circle with the kidnapping incident as the center and a radius of 2v·t kpd The circular area, and t kpd is the time taken by the mobile robot from the moment of kidnapping to the current moment, v is the running speed of the mobile robot, and D is the particle set L t The particle density in It is an empirical function of efficiency loss. When F / D is too large, F / D is appropriately reduced to ensure sampling efficiency. The efficiency loss empirical function is in the form of a piecewise function, and its specific value can be determined through experiments.

[0140] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A mobile robot positioning recovery method based on Monte Carlo positioning method, characterized in that: The steps include: S1, based on: the sensor measurement data Z of the mobile robot at the current time t t and odometer measurement data u t , and the particle set L at the previous moment t-1 t-1 , short-term measurement likelihood ψ st-1 and the long-term measurement likelihood ψ lt-1 ; Calculation results: particle set L at the current time t t , short-term measurement likelihood ψ s , long-term measurement likelihood ψ l and the average confidence ψ mean ; Furthermore, it is determined whether a kidnapping incident occurs with the mobile robot; if so, the process proceeds to step S2; if not, the process skips steps S2 to S4 and proceeds to step S5; S2. Calculate and obtain the particle set L t Student's distribution of the recovery auxiliary particles based on the positioning failure time S3, judging whether to send the particle set L to the particle set L according to the long and short time strategy t Add recovery auxiliary particles, and further determine that the recovery auxiliary particles are in the particle set L t to adjust the particle set L t The ratio of recovery auxiliary particles to ordinary particles in; S4, calculate and determine the particle set L t The number of recovery auxiliary particles M added in sd , and to the particle set L t Add recovery auxiliary particles; S5. The Student's distribution based on the positioning failure duration obtained in step S2 is For the particle set L t The recovered auxiliary particles in the sample are sampled; S6. According to the order importance, the particle set L t Sampling of ordinary particles in S7, combining the recovery auxiliary particles obtained by sampling and the ordinary particles to form a new particle set L t , and based on the particle set L t Steps S1 to S6 are performed in a loop to maintain the positioning of the mobile robot.

2. The method for mobile robot positioning recovery based on Monte Carlo positioning method according to claim 1, characterized in that: For any time t, the particle set is: in, is the particle set L t The position of the nth particle in at time t, is the particle set L t The weight of the nth particle in the particle set L, k is t The number of particles in For the previous moment t-1, the particle set is: in, is the particle set L t-1 The position of the nth particle in at the last moment t-1, is the particle set L t-1 The weight of the nth particle in the particle set L, k is t-1 The number of particles in .

3. The method for mobile robot positioning recovery based on Monte Carlo positioning method according to claim 2, characterized in that: In step S1, the particle set L at the current time t t , short-term measurement likelihood ψ s , long-term measurement likelihood ψ l and the average confidence ψ mean The specific calculation methods include: S1.

1. Express the confidence of the position of the mobile robot at any time t as: bel(l t )=P(l t |z t ,u t ,m); Among them, l t is the position of the mobile robot at the current time t, and l t ~(x t ,y t ,θ t ), Z t is the sensor measurement data of the mobile robot at the current time t, u t is the odometer measurement data of the mobile robot at the current time t, and m is the prior grid map data; S1.2, through the motion model P(l t |u t ,l t-1 ), predict the position l of the mobile robot at the current time t t , and the pose prediction confidence of the mobile robot at the current time t is expressed as: Among them, l t-1 is the position of the mobile robot at the previous moment t-1; S1.3, combining the sensor measurement data Z of the mobile robot at the current time t t , the pose confidence of the mobile robot at the current time t is expressed as: Among them, η is the normalization factor, P(z t |l t ,m) is the posture measurement model of the mobile robot; S1.

4. Constructing a particle set by a set of weighted particles The particle set L t The confidence level is expressed as: in, is the particle set L t The position of the nth particle in at time t, is the particle set L t The weight of the nth particle in L, δ is the Dirac-delta function, k is the particle set L t The number of particles in S1.4, according to the particle set at the previous moment t-1 The motion model P(l t |u t ,l t-1 ) Sampling to obtain the particle set L at the current time t t ; S1.5, through the posture measurement model P(z t |l t ,m) calculate and obtain the particle set L t The weight of each particle in And the weight Normalize and calculate the particle set L t The average confidence of each particle in mean ; S1.

6. Calculate the short-term measurement likelihood ψ using the following formula: s and the long-term measurement likelihood ψ l : ψ s =ψ s +t short (ψ mean -ψ s ); ψ l =ψ l +t long (ψ mean -ψ l ); Among them, τ short is the average decay rate of the Monte Carlo filter in a short period of time, τ long is the average decay rate of the Monte Carlo filter over a long period of time.

4. The method for mobile robot positioning recovery based on the Monte Carlo positioning method according to any one of claims 1 to 3, characterized in that: In step S1, the method for determining whether a kidnapping incident has occurred on the mobile robot is as follows: S1a, using information entropy Entropy (t) to represent the particle set L t The degree of particle dispersion in , the information entropy Entropy (t) is specifically: S2a. Preset a threshold value β of the information entropy. If the information entropy Entropy(t) satisfies Entropy(t)>β at any time t, it is determined that a kidnapping incident occurs to the mobile robot at the time t; if the information entropy Entropy(t) fails to satisfy Entropy(t)<β, it is determined that no kidnapping incident occurs to the mobile robot.

5. The method for mobile robot positioning recovery based on Monte Carlo positioning method according to claim 1, characterized in that: In step S2, the Student's distribution of the restoration auxiliary particles based on the positioning failure time is Specifically: Wherein, μ is the position of the mobile robot when the positioning data fails, n is the particle set L t The number of particles at the current moment, t kpd is the time elapsed by the mobile robot from the moment the kidnapping incident occurred to the current moment, v is the running speed of the mobile robot, Γ is the gamma function, h is the extreme distance condition of the mobile robot's movement, specifically h = n·S, and S is the circumference of the environment map.

6. The method for mobile robot positioning recovery based on Monte Carlo positioning method according to claim 1, characterized in that: Step S3 is specifically as follows: By max(0.0,1-ψ s / ψ l ) value to determine whether to add the particle set L t Add recovery auxiliary particles; And through 1-ψ s / ψ l Calculate the particle set L t The ratio of recovery auxiliary particles to normal particles in .

7. The method for mobile robot positioning recovery based on Monte Carlo positioning method according to claim 6, characterized in that: By max(0.0,1-ψ s / ψ l ) value to determine whether to add the particle set L t Add recovery auxiliary particles, including: If ψ s ≥ψ l , then max(0.0,1-ψ s / ψ l )=0, no need to add the particle set L t Add recovery auxiliary particles; If ψ s <ψ l , then max(0.0,1-ψ s / ψ l )=1-ψ s / ψ l , it is necessary to add the particle set L t Add recovery auxiliary particles.

8. The method for mobile robot positioning recovery based on Monte Carlo positioning method according to claim 1, 6 or 7, characterized in that: In step S4, the particle set L t The number of recovery auxiliary particles added is M sd , and specifically: M sd =θ(F / D)*F / D; Where F is a circle with the kidnapping incident as the center and a radius of 2v·t kpd The circular area, and t kpd is the time the mobile robot has experienced from the moment the kidnapping incident occurred to the current moment, v is the running speed of the mobile robot, and D is the particle set L t The particle density in the sample; θ(F / D) is the empirical function of efficiency loss. When F / D is too large, F / D is appropriately reduced to ensure sampling efficiency.

9. The method for mobile robot positioning recovery based on Monte Carlo positioning method according to claim 1, characterized in that: In step S5, if the restoration auxiliary particles are added to the obstacle area or unknown area of the map, the restoration auxiliary particles are resampled.

10. The method for mobile robot positioning recovery based on Monte Carlo positioning method according to claim 9, characterized in that: In step S6, when resampling the recovery auxiliary particles, the specific rules are as follows: With the kidnapping incident as the center, a circle with a radius of 2v·t is established. kpd If the unknown area occupies 3 / 4 or more of the circular judgment area, the position is evenly distributed. Take samples; If the area of the circular judgment area is greater than π, then the circular judgment area is uniformly distributed along the direction Sample the region U(-π,π).

Images