Localization in industrial robotics using rao-blackwellized particle filtering

This invention relates generally to industrial robotics, and more particularly to a probing-based method for probabilistic localization in industrial robotics.

A number of industrial robotics applications, including automatic robotic assembly, require navigation of a robotic arm to a desired pose with respect to an object or to the environment. This problem is often formulated as a peg-in-hole problem in which a needle-like probe has a single point of contact with an object that can include a hole, and in which the initial pose of the probe with respect to the object is unknown. The pose of the probe with respect to the object has six degrees of freedom (6-DOF), comprising 3-DOF uncertainty in a position of the probe and 3-DOF uncertainty in an orientation of the probe with respect to the object. The initial pose uncertainty may greatly exceed a desired accuracy in all 6-DOF.

Several conventional methods use probing and particle-based Monte-Carlo localization to solve robotic peg-in-hole problems. For example, one method describes a localization using probing and particle filtering for lock and key assembly. The method first densely probes the object with the probe at every (x, y) to obtain a contact position (x, y, z), generating a contact configuration-space map, which describes all possible poses in which the probe has contact with the object. After this preprocessing step of dense probing, the method performs particle filtering by probing the object sequentially with the probe, using the contact positions obtained from the sequential probing as the observations. The observation likelihoods are evaluated using the contact configuration-space map.

Another method similarly describes an exhaustive preprocessing step, densely probing an object with a probe that can have multiple points of contact with the object, and using a force/torque sensor connected to the probe to generate a force-torque map. The force-torque map includes contact force and torque at every possible pose at which there is contact between the probe and the object. This method also describes estimating the force-torque map directly from a computer-aided design (CAD) model. However, the estimated force-torque map is not as accurate as the map acquired by the probing. Using the force-torque map that was acquired in advance by dense probing, particle filtering is used with sequential probing to match the force/torque observations to the map. The method also optionally incorporates observations from a camera.

However, the aforementioned methods are not well suited for full 6-DOF localization, because number of particles required for standard particle filtering increases roughly exponentially with the number of dimensions in the search space. Although those methods are described for uncertainty in up to six degrees of freedom (6-DOF), in practice the methods are only used for localization in lower-dimensional search spaces, such as 2-DOF space in which uncertainty only exists in x, y translation.

Accordingly, it is desired to have a method that can efficiently determine a pose between a probe and an object, wherein the pose has up to six degrees of freedom (6-DOF). It is also desired to have a map representation that can be determined directly from a CAD model of the object without a preprocessing step of dense probing, and that can account for varying levels of uncertainty in the map and for discrepancies between the CAD model and the object.

It is an object of the current invention to provide a method for determining a pose of a probe with respect to an object by probing the object with the probe.

It is a further object of the invention to provide such a method that can handle differences between a model of the object and measurements of the object, e.g., arising from errors of fabrication and measurement.

Embodiments of the invention are based on a realization that particle filtering applied to an estimation of the 6-DOF pose of the probe is computationally difficult, but the 6-DOF pose can be factorized in two parts, i.e., 3-DOF location and 3-DOF orientation, such that the estimated orientation of the probe depends on the estimated location of the probe and can be updated based on that location using Kalman filtering. Based on this realization, the 6-DOF particle filtering is reduced to 3-DOF particle filtering and 3-DOF Kalman filtering conditioned on the location of each particle, which results in a more efficient computation. The theoretical framework of such factorization is known as Rao-Blackwellized particle filtering.

One embodiment of the invention discloses a method for determining a pose of a probe relative to an object by probing the object with the probe, wherein the pose includes a location of the pose and an orientation of the pose, wherein each probing results in a point of contact between the probe and the object, such that the pose is estimated at each point of contact, comprising the steps of: representing a probability of the pose between the probe and the object by a set of particles, wherein a probability of the location of the pose is represented by a location of each particle, and a probability of the orientation of the pose is represented by a Gaussian distribution over an orientation of each particle conditioned on the location of the particle; relocating each particle to a next location according to a motion of the probe from the point of contact to a next point of contact, wherein the motion is caused by the probing; determining an importance weight of each particle based on the next location of the particle, the probability of orientation of the particle, and a map model of the object, wherein the importance weight is a probability that the particle represents the point of contact; updating the probability of orientation of each particle in the set of particles using Kalman filtering conditioned on the next location of each particle; repeating the relocating, the determining, and the updating until particles in the set of particles concentrate around a particular pose; and determining the pose of the probe relative to the object based on the particular pose, wherein the steps of the method are performed by a processor.

Also, one variation of this embodiment determines the map model based on an object model and an uncertainty model, wherein the uncertainty model is determined by assigning an uncertainty function to each feature of the object model, wherein the feature is a face, an edge, or a vertex.

Another embodiment discloses a method for determining a pose of a probe relative to an object by probing the object with the probe, comprising steps of: determining a probability of the pose using Rao-Blackwellized particle filtering, wherein a probability of a location of the pose is represented by a location of each particle, and a probability of a orientation of the pose is represented by a Gaussian distribution over orientation of each particle conditioned on the location of the particle, wherein the determining is performed for each subsequent probing until the probability of the pose concentrates around a particular pose; and estimating the pose of the probe relative to the object based on the particular pose.

Yet another embodiment discloses a system configured for determining a pose of a probe relative to an object by probing the object with the probe, wherein the pose includes a location of the pose and an orientation of the pose, wherein each probing results in a point of contact between the probe and the object, such that the pose is estimated at each point of contact, comprising: means for representing a probability of the pose of the probe relative to the object by a set of particles, wherein a probability of the location of the pose is represented by a location of each particle, and a probability of the orientation of the pose is represented by a Gaussian distribution over an orientation of each particle conditioned on the location of the particle; means for relocating each particle to a next location according to a motion of the probe from one probing's point of contact to a next probing's point of contact; means for determining an importance weight of each particle based on the next location of the particle, the probability of orientation of the particle, and a map model of the object, wherein the importance weight is a probability that the particle represents the point of contact; means for updating the probability of orientation of each particle in the set of particles using Kalman filtering conditioned on the next location of each particle; means for repeating the relocating, the determining, and the updating until particles in the set of particles concentrate around a particular pose; and means for determining the pose of the probe relative to the object based on the particular pose, wherein the steps of the method are performed by a processor.

FIG. 1 is a schematic of a system and a method for determining a pose between a probe and an object according to the embodiments of the invention;

FIG. 2 is a block diagram of a method for determining a pose between a probe and an object according to the embodiments of the invention;

FIG. 3 is a schematic of probabilistic dependencies according to the embodiments of the invention;

FIG. 4 is a schematic of coordinate transformation between an object coordinate system and a base coordinate system according to the embodiments of the invention;

FIG. 5 is a schematic of a map model according the embodiments of the invention; and

FIG. 6 is pseudocode for determining a probability of the pose between the probe and the object using Rao-Blackwellized particle filtering according to the embodiments of the invention.

FIG. 1 shows a system 100 and a method 200 for determining a pose 150 of a probe 110 with respect to an object 120 by probing the object with the probe. In various embodiments of the invention, the probe is a touch probe or a touchless probe, e.g., an electrical, a mechanical, an optical, or a laser probe. Each probing determines a point of contact 115 between the probe and the object, and the pose is estimated at each point of contact. For example, in one embodiment, the probe is a mechanical probe having a needle-like shape, and each point of contact is a point at which the probe touches the object. In alternative embodiments, the probes are touchless probes and each point of contact is a location on a surface of the object determined by the touchless probes.

After the pose of the probe relative to the object is determined, the pose can be used in an industrial process, which can involve picking up the object, positioning the object, mating a part with the object, determining the pose of the robot with respect to the object, calibrating a robot coordinate system, or using a tool on the object, such as a spray gun, arc welder, drill, or grinder.

The object is represented by a map model 500 determined based on a three-dimensional (3D) model 125 of the object, such as a CAD model, and an uncertainty model 510 , as described in more detail below. The system also includes a processor 140 performing steps of the method 200 according to embodiments of the invention. The processor includes input/output interfaces and memories as known in the art.

The probe is positioned in a vicinity of the object, but the pose of the probe with respect to the object is unknown and has uncertainty up to six degrees of freedom (6-DOF), including a location of the pose and an orientation of the pose. Typically, the location and the orientation of the pose are each three-dimensional, but in some embodiments, the location and/or the orientation of the pose have less than 3-DOF uncertainty.

In one embodiment, the probing is achieved by repositioning a robot arm 160 that holds the probe, moving the probe in general direction of the object until detecting a contact between the probe and the object, and measuring the location 130 of a point of contact in a robot coordinate system. In various embodiments, the location of the contact is detected and measured by built-in functions and encoders of the robotic arm, by a compliance control of the robot arm, and/or by an external sensor, such as a camera, a touch sensor, or a force sensor.

Embodiments of the invention are based on a realization that particle filtering applied to an estimation of the 6-DOF pose of the probe is computationally difficult, but the 6-DOF pose can be factorized in two parts, i.e., 3-DOF location and 3-DOF orientation, such that the orientation of the probe depends on the location of the probe and can be updated based on that location using Kalman filtering. Based on this realization, the 6-DOF particle filtering is reduced to 3-DOF particle filtering and 3-DOF Kalman filtering, which results in a more efficient computation. The theoretical framework of such factorization is known as Rao-Blackwellized particle filtering.

FIG. 2 shows a block diagram of the method 200 according to the embodiments of the invention. According to the aforementioned realization, the relative pose between the probe and the object is determined using a Rao-Blackwellized particle filter, in which a probability distribution over location (3-DOF translation) is represented using particles, i.e., each particle represents a specific hypothesis about the location, and a distribution over orientation (3-DOF rotation) is approximated by a Gaussian distribution for each particle, conditioned on the location of the particle. This factorization greatly improves the efficiency of the filtering, resulting in an orders-of-magnitude reduction in the number of particles required as compared to conventional particle filtering.

Therefore, a probability of the pose of the probe relative to the object is represented by a set of particles 210 , wherein a probability of the location of the pose is represented by a location 212 of each particle, and a probability of the orientation of the pose is represented by a Gaussian distribution 215 over orientation for each particle conditioned on the location of the particle.

For each subsequent probing 220 , a motion 225 of the probe is acquired, the location of each particle is updated 230 to a next location 235 according to the motion, and an importance weight 255 of each particle is updated 250 based on the next location of the particle, the Gaussian distribution over orientation of each particle, and the map model 500 of the object, wherein the importance weight is a probability that the location of the particle represents the point of contact 115 and that the orientation distribution of the particle represents the orientation of the probe with respect to the object. Also, the Gaussian distribution 215 over orientation of each particle is updated 240 using Kalman filtering 245 , e.g., extended Kalman filtering (EKF), conditioned on the next location of the particle. The EKF produces a next estimate of probability distribution 258 over orientation conditioned on the next location of the particle.

If the particles in the set of particles 210 are concentrated 270 around a particular pose, the pose 150 of the probe relative to the object is determined based on that particular pose, e.g., averaging locations and the orientation of the particles. Otherwise, the repositioning, probing, and updating steps of the method 200 are repeated.

In one embodiment, a decision 265 is made whether to use the updated set of particles and importance weights for a subsequent probing, or to resample 260 the set of particles with probability proportional to the importance weight of each particle. If the set of particles is resampled, then with high probability, the particles with low importance weights will be removed from the set 210 and the particles with high importance weights will be represented multiple times in the set.

In one embodiment the map model of the object is generated based on the model of the object that is being probed, e.g., a computer-aided design (CAD) model, as well as based on an uncertainty model that represents any prior knowledge about the uncertainty of the CAD model. The uncertainty model incorporates uncertainty about different features of the object. Therefore, the map model is a probabilistic map model.

Usage of the map model according the embodiment of the invention is advantageous in situations in which actual measurements are not consistent with the CAD model. Such differences can arise from measurement errors due to numerous factors such as slipping and sensor imprecision, as well as from differences between the CAD model of the object and an actual shape of the object.

Probabilistic Dependencies

The pose of the probe relative to the object is represented as (s, θ), where s=(x, y, z) T and θ=(α, β, γ) T are relative positions and orientations, respectively, between the probe and the object. Angles (α, β, γ) are defined as angles of rotation around each of the (x, y, z) axes.

FIG. 3 shows a graphical model of probabilistic dependencies used in the method 200 . A sequence 310 of the motion of the probe from each point of contact to a next point of contact, wherein the motion is caused by each probing from time 1 to t, is defined as u 1:t ={u 1 , . . . , u t }, where u i represents the motion from the point of contact at time i−1 to the point of contact at time i. A sequence 330 of contact events is defined as Z 0:t ={Z 0 , . . . , Z t }. The embodiments factorize the probability of the pose of the probe relative to the object according to

p ( S 0:t ,θ|z 0:t ,u 1:t )= p ( S 0:t |Z 0:t ,u 1:t ) p (θ| S 0:t ,Z 0:t ).  (1)

Accordingly, each particle represents a sequence of positions S 0:t 320 of the points of contact and a Gaussian distribution 340 over the orientation between the probe and the object. The Gaussian distribution is represented by a mean μ t and a covariance matrix Σ t .

Typically, the contact event is binary, Z t ε{1, 0}, where Z t =1 if at a time instant t the probe has a contact with the object at position S t , and Z t =0 otherwise. However, the contact position is measured only at the time instant when the point of contact is sensed, i.e., Z t =1 for each probing. Accordingly, for clarity, the probabilistic notation is simplified by representing p(Z t =1) using a shorthand p(Z t ).

Coordinate Transformations

FIG. 4 shows a coordinate transformation between an object coordinate system 410 , which is fixed with respect to the object, and a base coordinate system 420 , which is fixed with respect to a robot coordinate system, i.e., a frame of reference of the robotic arm (not shown). In some embodiments, the transformation between the robot coordinate system and the base coordinate system is a translation, without rotation, such that the motion of the probe is represented by the same translation in the base coordinate system as in the robot coordinate system.

The position of the probe at time t in the base coordinate system is defined as S t . The position of the probe at time t in the object coordinate system is defined as S t O . The base coordinate system and the object coordinate system are related by a 3D rotation 430 θ about an initial position S 0 440 of the probe. Thus, the initial position of the probe is the same in both the base and object coordinate systems. However, the orientation difference, θ, between the two coordinate systems is unknown.

The map model 460 and the measurement probability are defined in the object coordinate system. At time t=0, the base and object coordinate systems are identical. At time t=1, after the motion 445 of the probe, the Gaussian distribution over orientation of each particle is updated, corresponding to a rotation of the object coordinate system with respect to the base coordinate system.

Motion

Each subsequent probing moves the probe from one point of contact to another. For each point of contact, the position of the probe in the robot coordinate system can be obtained from internal encoders of the robot arm. The motion of the probe from time t−1 to time t is identical in the robot coordinate system and in the base coordinate system. Using the motion u t , the position of each particle in the base coordinate system is updated from the position S t-1 to the next position s t according to

where N(μ, Σ) represents the multivariate normal (Gaussian) distribution with mean μ and covariance matrix Σ, and σ x , σ y , σ z model the noise due to errors of robot motion or errors in the values obtained from the encoders of the robotic arm.

Some embodiments use values of σ x , σ y , σ z that are larger than the actual robot motion errors and encoder errors, to account for particle deprivation, wherein none of the particles has an exactly correct estimate of the initial contact position, so that the noise in the motion model enables the particle filter to compensate for this inaccuracy over time.

The position S t O of the probe at time t in the object coordinate system is determined based on a rotation matrix R using the orientation θ=(α, β, γ) T . The rotation matrix R is determined according to

R ⁡ ( θ ) = ( c γ ⁢ c β c γ ⁢ s β ⁢ s α - s γ ⁢ c α c γ ⁢ s β ⁢ c α + s γ ⁢ s α s γ ⁢ c β s γ ⁢ s β ⁢ s α + c γ ⁢ c α s γ ⁢ s β ⁢ c α - c γ ⁢ s α - s β c β ⁢ ⁢ s α c β ⁢ c α ) , ( 3 )

where S α and C α are shorthand for sin α and cos α, respectively.

Accordingly, the position of the probe in the object coordinates at time t is determined according to

S t O =R (θ)( S t −S 0 )+ S 0 .  (4)

Map Model

Some embodiments of the invention are based on a realization that a measurement model of the object should reflect uncertainties of object model design and measurement, e.g., differences between the object model and the actual shape of the object and imperfection in measurement tools and procedures. For example, the tip of the probe might be rounded or wide rather than sharp and needle-like, which would lead to measurement errors. Accordingly, we determine and include into the map model parameters of uncertainties determined for every feature of the model, such as a face (plane), an edge (line), and a vertex (point).

FIG. 5 shows one example of a map model 500 according to the embodiments of the invention. The map of the object is determined from the object model 125 and the uncertainty model 510 . In some embodiments, the object model is generated from a CAD model of the object. The object model includes features f i 520 of the shape of the object, e.g., vertices, edges, and triangular faces. For example, as shown in FIG. 5 , the features f 1 , f 3 , f 5 represent vertices or edges, and the features f 2 , f 4 represent edges or faces.

In different embodiments, the uncertainty model is formulated using a wide range of probability fields or energy functions. For example, in one embodiment, the uncertainty model is formulated using Gaussian probability density functions of distances 545 between the points of contact 115 and the features 520 of the object model. The map model can assign different uncertainty values to different features of the object model, which enables users to incorporate knowledge of particular industrial settings or manufacturing processes into the map model.

The measurement uncertainties form the uncertainty model 510 . For each feature f k , where k={1, . . . , K}, a standard deviation of measurement uncertainty of the feature is defined as σ k 540 . In one embodiment, larger uncertainty is assigned to the features, such as the edges and the vertices at intersection of planes, whose normals form large angles with each other. For example, in one variation of this embodiment, the maximum angle between the normals of all possible pairs of the faces that border each edge/vertex is determined, and the uncertainty is assigned based on the value of the maximum angle.

The measurement probability is determined based on the distance 545 between the object model and the point of contact S O 530 in the object coordinates. The distance from the point of contact to each feature f k is defined as d(s O , f k ). In one variation of this embodiment, the function d computes the Euclidean distance between the point of contact s O and the feature f k . The distance to the face or the edge is determined if the perpendicular projection of the point of contact to the corresponding plane or line lies within that face or the edge.

For each point of contact, the distances to all features are determined and the feature with the smallest distance normalized by the corresponding standard deviation is selected as a contact feature. Thus, an index of the contact feature, k c , is determined according to

In one embodiment, a distance measure h is expressed either in the object coordinates as a function of S O or in the base coordinates as a function of the position s and the orientation θ, such that

h k c ( s ,θ)= d ( S O ,f k c ).  (6)

The measurement probability at time t for each particle is determined based on the distance measure according to

p ( Z t |S t ,θ)= N ( h k c ( S t ,θ);0,σ k c 2 ).  (7)

The notation N(X; μ, Σ) represents the multivariate normal (Gaussian) distribution over the variable x with mean μ and covariance matrix Σ.

Rao-Blackwellized Particle Filtering

FIG. 6 shows pseudocode for determining a probability of the pose of the probe relative to the object using Rao-Blackwellized particle filtering, wherein a probability of the location of the pose is represented by a location of each particle, and a probability of the orientation of the pose is represented by a Gaussian distribution over orientation of each particle conditioned on the location of the particle.

For each probing at time t, each particle j is associated with the position of the point of contact S t [j] between the probe and the object, as well as with the normal distribution over the orientation

p (θ)= N (θ;μ t [j] ,Σ t [j] ).

Initialization

The method 200 samples J particles from a uniform distribution over the position (x, y) in base coordinates, bounded by a maximum initial (x, y) uncertainty. In one embodiment, the initial value of the z coordinate for each particle j is determined such that the initial particle position S 0 [j] =(x, y, z) is set on the surface of the map model. For each particle, the Gaussian distribution over the orientation θ is initialized with mean μ 0 [j] =0 and a diagonal covariance matrix Σ 0 [j] representing the initial uncertainty.

Particle Update for Each Probing

The set of particles at time t is defined as X t . When the robot arm moves the probe from one contact position to the next, the position of each particle j={1, . . . , J} is updated based on the motion u t in the base coordinates obtained from the robot arm encoders and the observation Z t that the robot detects a contact. Based on each particle's next position, the distribution over the orientation θ for each particle is updated.

Position Update

The next position S t [j] of particle j at time t is determined using Equation (2), based on the position of the particle S t-1 [j] and the motion u t according to

S t [j] ˜p ( S t |S t-1 [j] ,u t )  (8)

Orientation Update

Based on the next position of the particle S t [j] in base coordinates, and the observation Z t that there was a contact between the probe and the object, the posterior distribution for each particle over the orientation θ is determined using EKF. One embodiment expresses the measurement probability as a function of the orientation and linearizes that measurement probability about θ=μ t-1 [j] , i.e., the mean of the distribution of the orientation for the particle determined after the previous probing.

To linearize the measurement probability about θ=μ t-1 [j] , the predicted position Ŝ t O[j] of each particle in the object coordinate system is determined based on the mean and Equation (4) according to

Ŝ t O[j] =R (μ t-1 [j] )( S t [j] −S 0 [j] )+ S 0 [j] .  (9)

The predicted particle position Ŝ t O[j] is used to determine the index of the contact feature, k c according to Equation (5).

We use the measurement probability (7) defined with respect to the contact feature, f k c , to update the posterior probability over angle, which is the second factor on the right hand side of (1).

Accordingly, the posterior probability further factorizes as

The Equation (10) is used to approximate the posterior probability of the orientation as a Gaussian distribution. One embodiment uses EKF by linearizing the measurement function about θ=μ t-1 [j] according to

h k c ⁡ ( s t [ j ] , θ ) ≈ ⁢ h k c ⁡ ( s t [ j ] , µ t - 1 [ j ] ) + ∂ h k c ∂ θ ⁢ ( s t [ j ] , μ t - 1 [ j ] ) ⁢ ( θ - μ t - 1 [ j ] ) = ⁢ h ^ k c , t [ j ] + H t [ j ] ⁡ ( θ - μ t - 1 [ j ] ) ( 11 )

where ĥ k c , t [j] =d(Ŝ t O[j] , f k c ) is the distance measure determined at the predicted position Ŝ t O[j] , and H t [j] the Jacobian of the distance measure with respect to the orientation θ determined according to

The posterior distribution over the orientation is then computed using standard EKF measurement update rule, according to

K t [j] =Σ t-1 [j] H t [j] T ( H t [j] Σ t-1 [j] H t [j] T +σ k c 2 ) −1   (13)

μ t [j] =μ t-1 [j] −K t [j] ĥ k c ,t [j]   (14)

Σ t [j] =( I−K t [j] H t [j] )Σ t-1 [j]   (15)

Importance Weight Update and Resampling

At each time t, the importance weight w t [j] of each particle j is determined from the particle's previous importance weight w t-1 [j] and the measurement probability. In one embodiment of the invention, the importance weight is updated by marginalizing the measurement probability over the orientation of the particle according to

Accordingly,

w t [j] ∝w t-1 [j] ·(2 πq t [j] ) −1/2 exp(−( ĥ k c ,t [j] ) 2 /2 q t [j] ),  (17)

where

q t [j] =H t [j] Σ t-1 [j] H t [j] T +σ k c 2 .  (18)

To keep the diversity of particles at a reasonable level, one embodiment estimates, for each probing, an effective number J eff of particles according to

If J eff is less than a threshold determined based on a number of particles in the set of particles, e.g., J eff <J/2, the particles are resampled 260 with a probability proportional to the weights w t [j] of the particles. After resampling, all particle weights are reset to w t [j] =1/J.

If the particles are not resampled at time t, e.g., if J eff ≧J/2, then all particles with the importance weights determined according to Equation (17) are preserved in the set of particles 210 .

In one embodiment, a relatively large number of particles are used initially in the set 210 . After several iterations of the method 200 , however, the number of particles required for localization decreases. The number of particles is reduced at each resampling, until the number of particles reaches a predefined number, J min . One variation of this embodiment reduces the number of particles in the set by half, by resampling only half as many as the current number of particles, if the number of particles J is greater than J min before the resampling.

Concentration

Embodiments of the invention repeat the probing and the resampling of the set of particles until the particles in the set of particles concentrate around a particular pose. In one embodiment, the concentration is determined based on a weighted average and a weighted covariance of the positions of the particles in the object coordinate system according to

s _ t o = ∑ j = 1 J ⁢ ⁢ w t [ j ] ⁢ s t o ⁡ [ j ] , and ( 20 ) Ψ t o = ∑ j = 1 J ⁢ ⁢ w t [ j ] ⁡ ( s t o ⁡ [ j ] - s _ t o ) ⁢ ( s t o ⁡ [ j ] - s _ t o ) T 1 - ∑ j = 1 J ⁢ ⁢ ( w t [ j ] ) 2 ( 21 )

where S t O[j] is determined using Equation (4) according to

S t O[j] =R (μ t [j] )( S t [j] −S 0 [j] )+ S 0 [j] .  (22)

The probing is repeated until the trace of the covariance matrix Ψ t O is less than a predetermined threshold indicating that uncertainty among particle positions is small. Once this convergence condition is achieved, the estimated position of the pose is determined based on the weighted average of the positions of all particles remaining in the set. The estimated orientation of the pose is determined based on a weighted average of the mean orientation of the particles.

The determined pose can be directly used for subsequent industrial processes. The pose can be further refined by using the determined pose as the initial estimate for a pose estimation algorithm such as iterative closest point, which minimizes a cost function by using all the points of contact simultaneously.

Although the invention has been described with reference to certain preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the invention. Therefore, it is the object of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the invention.