A high-precision object pose estimation and fine manipulation method based on visual-tactile fusion

Abstract

The present invention provides a high-precision object pose estimation and fine manipulation method based on visual-tactile fusion, which obtains visual and tactile information through the physical interaction between the robot and the environment, thereby completing accurate pose estimation in dynamic scenes; the present invention constructs a hand object pose correction algorithm based on particle filtering, which can estimate the pose change when the held object slides and the contacted object moves. This method is suitable for most high-precision operation scenarios, including some challenging scenarios with dynamic attribute changes, and significantly improves the robot's ability to cope with dynamic interactive environments; the present invention performs high-precision operation tasks on 13 objects with different geometric shapes, including the FMB benchmark. The results show that our method achieves accurate pose estimation with a translation error of 1-2 mm and a rotation error of about 2 degrees. At the same time, it can cope with dynamic situations such as object sliding.

Description

Technical Field

[0001] The present invention belongs to the field of robotics technology, and in particular relates to a high-precision object pose estimation and fine manipulation method based on vision-tactile fusion. Background Art

[0002] Traditional methods for object pose estimation primarily rely on visual information, performing feature extraction and matching based on image preprocessing results, ultimately obtaining an estimated object pose based on the matched feature points. Recent advances in tactile sensor research have enabled them to complement vision and enhance pose estimation accuracy. Strategies for achieving high-precision pose estimation by combining natural perception modalities of vision and touch can be broadly categorized into two main categories. One common strategy utilizes multimodal data fusion, extracting information such as point clouds from tactile data and fusing it with visual data using optimization algorithms or neural networks to obtain a compact and efficient multimodal representation, thereby enhancing pose estimation accuracy. Another research approach uses visual information to make a preliminary pose estimate, then fine-tunes and optimizes it using techniques such as particle filtering combined with tactile data to achieve high-precision pose estimation.

[0003] Due to factors such as noise and object occlusion, methods that rely solely on visual information often produce low-precision object pose estimation results. Tactile perception can serve as a complementary modality to vision, enabling more accurate pose estimation based on both visual and tactile modalities. However, most of these visual-tactile based methods assume that the position of the gripped object remains unchanged during robot operation, or that the contacted object remains absolutely stationary. They lack the ability to adapt to environmental changes, such as when the object slides or moves. Some studies consider object sliding, first using visual information to obtain a rough pose result, and then using tactile information to make adjustments. However, they typically only detect the initial slip or adjust the gripping force based on the initial slip, ignoring the prediction of the object's pose change after sliding. These research methods are not applicable in the dynamic operating environment of the robot. Summary of the Invention

[0004] In view of this, the purpose of the present invention is to provide a high-precision object pose estimation and fine manipulation method based on vision-tactile fusion.

[0005] A high-precision object pose estimation and fine manipulation method based on visual-tactile fusion, comprising:

[0006] Step 1: Define the object held by the robot end effector as the "slave object" and the object being touched as the "master object";

[0007] Estimate the initial poses of the master and slave objects respectively, and track the pose changes of the master object in real time. Then, transfer the pose estimation error of the slave object relative to the master object to the pose estimation error of the slave object relative to the robot end-effector, thereby obtaining the corresponding robot end-effector pose.

[0008] Step 2: When estimating the position of the slave object relative to the robot end effector, consider that the slave object may slip in the robot end effector and estimate the position change of the slave object in the robot end effector coordinate system.

[0009] Step 3: Use particle filtering technology to obtain accurate pose estimation of the object relative to the robot's end effector through multiple interactions between the robot and the environment. Specifically,

[0010] Step 31: First use the FoundationPose method to estimate and track the pose x of the object in real time. est , sampling is performed in a uniform distribution within the set range to obtain the initial particle swarm;

[0011] Step 32: For the jth particle in the current particle swarm, update the posture as follows:

[0012]

[0013] Where ν is the systematic error; is the posture of the jth particle at time T-1; It is the posture at time T obtained by updating the posture at time T-1;

[0014] Step 33: Add small noise to the particles of this round and record the position of the robot end effector relative to the world coordinate system at the current moment. and the pose of the main object relative to the world coordinate system 3D space model of master object and slave object m and O s , and input them into the collision simulation experiment to calculate the particle likelihood value size:

[0015]

[0016] in, represents the distance between the master and slave objects calculated from the collision simulation experiment of the jth particle, d maxth and d minth The maximum and minimum distances between the master and slave objects are respectively set; y T Indicates the state observation model result, representing the contact degree between the master object and the slave object, Represents the migration process from the situation of robot and environment contact to simulation, e c is the error caused by contact action; x T Represents the posture vector of the particle swarm;

[0017] Step 34: normalize the likelihood value of each particle and use it as the weight of the particle;

[0018] Step 35: Sample each particle according to the weight distribution of the current particle swarm to generate a new particle swarm; if the iterative update stop condition is not met, return to step 32 to continue updating the particle swarm; if the iterative update stop condition is met, perform weighted summation on the postures of all particles in the current particle swarm based on the weights to obtain the final output of the slave object posture.

[0019] Preferably, in step 1, the method for obtaining the corresponding robot end effector posture includes:

[0020] First, use the FoundationPose method to estimate the initial pose results of the master object and the slave object respectively and Then the true values ​​of the two poses are expressed as and where e m and e s Represent the errors of the pose estimation of the master object and the slave object respectively; then, define the relative pose of the slave object with respect to the master object And the two error terms e m and e s Integrate into an overall error term e o In the total error term e o Refers to the overall error in the pose estimation from the object and the main object, and the formula is as follows:

[0021]

[0022] e o =(e m ) -1 e s

[0023] In the above formula, the pose of the object in the world coordinate system By obtaining the pose of the object relative to the robot's end effector and the pose of the end effector in the world coordinate system To estimate; from the pose of the object relative to the main object Further written as After using tactile information for pose correction, based on the path points of the slave object in the master object coordinate system The corresponding robot end effector pose can be obtained as follows:

[0024]

[0025] Preferably, in step 2, the pose change T of the object in the robot end effector coordinate system is estimated. t s The methods include:

[0026] First, a smoothing filter is applied to the raw data to remove edges and some noise points, and then the feature points around the contact surface are calculated;

[0027] Based on the calculation and extraction of feature points, the real-time displacement of the center of the contact area is calculated according to the features, and finally the translation slip T of the object in the robot end effector coordinate system at this moment is calculated. tran =||vw||2, where v and w represent the centers of the two contact areas before and after sliding, respectively;

[0028] The rotational slip T is inferred from the uneven distribution of the marker rotation angle along the rotation center. rota ;

[0029] Finally, the hand object pose change estimate T t s By translation slip T tran and rotational slip T rota When the sliding is terminated, the calculation of its pose change is stopped.

[0030] Preferably, the rotational slip is caused by The calculation shows that the atan(y,x) function returns the angle between the line connecting the point (x,y) and the origin (0,0) and the positive direction of the x-axis, which can be used to obtain the corresponding rotational posture change; where n represents the number of sampled markers, and (xn,yn) represents the coordinates of marker point n relative to the sensor origin.

[0031] The present invention has the following beneficial effects:

[0032] The present invention provides a high-precision object pose estimation and fine manipulation method based on visual-tactile fusion, which obtains visual and tactile information through the physical interaction between the robot and the environment, thereby completing accurate pose estimation in dynamic scenarios; the present invention constructs a hand object pose correction algorithm based on particle filtering, which can estimate the pose change when the held object slides and the contacted object moves. This method is suitable for most high-precision operation scenarios, including some challenging scenarios with dynamic attribute changes, and significantly improves the robot's ability to cope with dynamic interactive environments; the present invention performs high-precision operation tasks on 13 objects with different geometric shapes, including the FMB (Functional Manipulation Benchmark) benchmark. The results show that our method achieves accurate pose estimation with a translation error of 1-2 mm and a rotation error of approximately 2 degrees, and can also cope with dynamic situations such as object sliding. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 Schematic diagram of the method framework of the present invention;

[0034] Figure 2 To compare the experimental settings, the bottom row shows the actual scene of the robot operation;

[0035] Figure 3 To compare the effects: pose estimation accuracy of the action selection module (a) and the weight update module (b), where the error value is calculated based on the distance metric △t+0.1△R;

[0036] Figure 4 For comparison purposes: As can be seen from these figures, the method proposed in this invention has made significant improvements in ensuring rapid convergence of each degree of freedom (taking the particle position update process as an example). DETAILED DESCRIPTION

[0037] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0038] The robot in this invention has the ability to autonomously grasp an object and can operate autonomously, using that object to contact another object in the environment. To improve applicability, the object held by the robot's end effector is called the "slave object," and the object being contacted is called the "master object."

[0039] Before introducing the technical solution in detail, let's first explain the meaning of each parameter or symbol: in the formula, the slave object (slave) is represented by s; the master object (master) is represented by m; the world coordinate system (world) is represented by w; the robot end effector (gripper) is represented by g;

[0040] Based on the 3D displacement field and 3D distributed force of the tactile sensor, the robot kinematics, the known 3D model of the object and the RGB-D information, the method of the present invention aims to estimate the accurate relative pose of the slave object with respect to the master object in real time. This helps robots perform high-precision manipulation tasks. The object pose describes the position and posture of an object in a given coordinate system in 3D space, such as translation (T x ,T y ,T z ) and quaternions (R x ,R y ,R z ,R w ), which helps to uniformly add noise in the subsequent particle filtering. In the present invention, tactile measurement refers to recording tactile information such as the 3D displacement field and 3D distributed force when contact is detected during the interaction between the robot and the environment.

[0041] like Figure 1 As shown, (a) the present invention first uses the FoundationPose method (Unified 6D PoseEstimation and Tracking of Novel Objects, object 6D pose estimation and tracking method) to estimate the initial poses of the master object and the slave object respectively (both are relative to the world coordinate system), and tracks the pose change process of the master object in real time, and then transfers the pose estimation error of the slave object relative to the master object to the pose estimation error of the slave object relative to the robot end effector. (b) In the pose estimation of the slave object relative to the robot end effector, since the slave object may slip in the robot end effector, the present invention proposes a real-time module to estimate the pose change (rotation and translation) of the slave object in the robot end effector coordinate system. (c) Finally, based on the previous pose change estimation of the slave object relative to the robot end effector, the present invention uses particle filtering technology to obtain an accurate pose estimation of the slave object relative to the robot end effector through multiple interactions between the robot and the environment.

[0042] 1. Relative pose transformation of dynamic scene objects

[0043] like Figure 1 As shown in (a), this module aims to concentrate the pose estimation errors of the master object and the slave object into the pose estimation of the slave object relative to the robot end effector. First, the present invention uses the FoundationPose method to estimate the initial pose results of the master object and the slave object respectively. and Their relationship to truth value is and Where e represents the error of the pose estimation, and the present invention will track the pose change process of the master object in real time. Then, the present invention defines the relative pose of the slave object relative to the master object And the two error terms e m and e s integrated into an overall error term e o , the overall error term e o Refers to the total error of the pose estimation from the object and the main object (the total error term e o This is just an intermediate result. The error term e with actual physical meaning will be given later. o '), the formula is as follows:

[0044]

[0045] e o =(e m ) -1 e s (2)

[0046] In the above formula, the pose of the object in the world coordinate system The pose of the object relative to the robot end effector can be and the pose of the end effector in the world coordinate system Therefore, the pose of the object relative to the main object It can be further written as The error e o 'It only needs to be considered in the pose estimation of the slave object relative to the robot end effector. After the pose correction is performed using tactile information, the position of the slave object is corrected based on the path points in the master object coordinate system. (p refers to the path point), the corresponding robot end effector pose can be obtained as

[0047]

[0048] End effector using real-time computing The robot is able to perform high-precision manipulation tasks under various dynamic environmental conditions, including scenarios where the slave object slides and the master object moves, demonstrating its adaptability and precision in dynamic environments.

[0049] Estimate the pose of the slave object relative to the master object Error transfer from the object pose estimate relative to the robot end effector After the error is corrected, the present invention can focus on solving the pose result of the object relative to the robot end effector. However, since the slave object may slip in the robot end effector, the present invention proposes a real-time module to estimate the position change (rotation and translation) T of the slave object in the robot end effector coordinate system. t s , where the superscript refers to the object s and the subscript refers to the current time t.

[0050] 2. Estimation of hand object pose change T t s

[0051] During robot operation, the smooth surface of the tactile sensor or frequent object sliding caused by collisions between the robot and the environment may cause grip uncertainty in the robot's motion. Therefore, the present invention integrates the estimated errors of the master and slave object posture changes into the posture estimation of the slave object relative to the robot's end effector. It is first necessary to estimate the posture change of the slave object when it slides on the robot's end effector, and then enter the particle filter stage to estimate the final result. The tactile sensor can detect the sliding signal by analyzing the motion distribution of the marker points on the sensor surface. The sliding starts from the peripheral contact area, causing relative displacement of the sensor and the object surface in the peripheral area, while the central area remains adhered to the contact surface until full sliding occurs. Therefore, a significant difference in motion between the peripheral and central markers means active sliding, which helps to estimate the posture change of the hand from the object.

[0052] like Figure 1 As shown in (b), the present invention uses the 3D displacement field of the tactile sensor as input and identifies the degree of partial sliding based on the unevenness of the displacement distribution of the markers on the sensor. The present invention first applies a smoothing filter to the raw data to remove edges and some noise points, and then calculates the feature points around the contact surface. Based on the computational extraction of the feature points, the present invention calculates the real-time displacement of the center of the contact area based on the features, and finally calculates the displacement T of the object at this moment in the robot end effector coordinate system. tran =||vw||2, where v and w represent the centers of the two contact areas before and after sliding, respectively. In addition to translational slip, the present invention can also infer rotational slip from the uneven distribution of the rotation angle of the marker along the rotation center. The rotation angle of the marker in the peripheral area is much smaller. Rotational slip can be calculated by The calculation results show that the atan(y,x) function returns the angle between the line connecting the point (x,y) and the origin (0,0) and the positive direction of the x-axis, which can be used to obtain the corresponding rotation posture change. Finally, the hand object posture change estimate T t s By translation slip T tran and rotational slip T rota When the sliding is terminated, the calculation of its pose change is stopped.

[0053] After estimating the pose change (rotation and translation) of the slave object in the robot end effector coordinate system, the present invention can use particle filtering technology to estimate the static pose of the slave object in the robot end effector coordinate system, and obtain accurate pose estimation results through multiple interactions between the robot and the environment.

[0054] 3. Contact-based hand-object posture correction

[0055] Robot system model: Considering the change in the hand object posture caused by the possible sliding of the slave object, the hand object posture refers to the posture of the slave object in the robot end effector coordinate system. The present invention can obtain the posture change T of the slave object at the current time t in real time. t s Therefore, the state space model can be written as follows

[0056] System model: x t =T t s x t-1 +ν(4)

[0057] Contact state model:

[0058] where ν and e c are the system error and the error caused by contact action respectively. The position of the robot end effector relative to the world coordinate system at time t is x t Indicates the position represented by each particle at time t in the particle filter (i.e., the state in the state space model: the hand object position), the 3D space models of the master object and the slave object are O m and O s ,function Represents the migration process from the situation of contact between the robot and the environment to the simulation. It is represented by y t The state observation model result of (∈[0,1]) represents the contact degree between the master object and the slave object. The closer it is to 1, the higher the contact degree. The goal of using particle filtering to correct the hand object posture is to obtain a set of measurement values ​​based on the interaction between the robot and the environment. Iteratively estimate the hand object pose x t The above can be formulated in a Bayesian setting and estimated using a particle filter as follows:

[0059] (1) Initial pose hypothesis generation: After concentrating the pose errors of the master and slave objects in the hand pose estimation, the present invention uses particle filtering to generate and maintain a set of potential candidate hypotheses about the hand pose. The present invention first uses the FoundationPose method to estimate and track the pose x of the slave object in real time. est, the present invention samples uniformly distributed samples within a reasonable range, thereby initializing the particle swarm (for example, from [x est -x thres ,x est +x thres ] sample out x), where the threshold x thres Affects the rate of weight update in particle filtering.

[0060] (2) Theoretical Basis: In particle filtering, the present invention obtains a new particle swarm in each round of update. Specifically, a new particle swarm is obtained by sampling the particle swarm according to the particle weights, and then proceeds to the next iterative update. The theoretical basis for sampling for the next iterative update is explained below. At time T, based on the observation results at time T-1, the present invention can derive the following posterior probability:

[0061]

[0062] For the above formula, Refers to the posterior probability at time T, which is proportional to and and Refers to the predicted value at time T-1, which is ultimately approximated to The weighted sum of .

[0063] In formula (6)

[0064]

[0065] Among them, formula (7) represents the system model, formula (8) is the new particle swarm obtained by sampling at time T-1, and N represents the total number of N particles in the particle swarm.

[0066] (3) Update of pose hypothesis: Figure 1 As shown in (c), during the contact process, the robot gradually approaches the master object until the tactile sensor detects a change in three-dimensional force reaching a certain threshold. The robot strategically selects the contact area between the slave object and the master object to minimize the number of interactions between the two objects. Based on the simulated collisions, each contact can determine some degrees of freedom. This helps select the contact area with the most information, and the next contact can then be used to determine the remaining undetermined degrees of freedom.

[0067] The present invention updates the likelihood value of each pose particle by calculating the distance between the primary object and the secondary object under the current pose hypothesis. In each round of iteration, the present invention first adds a small noise to the particles of this round, and then the present invention records the pose of the robot end effector relative to the world coordinate system at time T. and the pose of the main object relative to the world coordinate system They are input into the collision simulation environment to calculate the particle likelihood value, where the collision simulation refers to the conversion function h in the state space model. Specifically, the absolute value of the geometric distance Particles larger than a certain threshold are discarded, where j represents the jth particle in the particle sequence. In order to calculate the distance between the master object and the slave object, the present invention uses FCL (Flexible Collision Library) to simulate. in will be given by the system model in the state space model, O m ,O s , is the required input for distance calculation (defined above). Geometric distance The negative distance that may represent the collision depth can also be replaced by the collision volume calculation to further calculate the size of the particle likelihood value. The present invention provides the following likelihood value calculation process:

[0068]

[0069] Further explanation of the above formula: in order to improve the performance efficiency of weight update in particle filtering, the present invention will assign weights according to the absolute value of the geometric distance. In theory, Figure 1 As shown in (c), a smaller geometric distance corresponds to a larger likelihood value of 0.9, indicating that the particle is more important than the particle with a likelihood value of 0.1 because the particle is closer to the contact situation between the master object and the slave object. minth The particle indicates that the two objects are in contact and is assigned 1. On the contrary, the geometric distance exceeds the maximum threshold d maxth The particles with the minimum threshold d will be ignored. minth and the maximum threshold d maxth The distance value between the two, the present invention will use the formula Calculated, where Refers to the distance value corresponding to the jth particle at time T. Finally, the present invention can normalize the particle likelihood value to obtain the weight of each particle at time T, that is, the proportion of the likelihood value of each particle in the sum of the likelihood values ​​of the entire particle group:

[0070]

[0071] Weight distribution based on particle swarm The present invention generates new particle groups through weighted sampling to continuously update particle distribution.

[0072] (4) Accurate pose results based on termination conditions: After each round of updating particle weights, the present invention calculates the six-degree-of-freedom pose errors (translation and rotation) of all particles. When the six component deviations of the pose are all less than the specified threshold T thres Time: max 1≤i≤6,1≤j<k≤N |x ji -x ki | <T thred , the particle filtering process will terminate, and this threshold setting will be related to the overall pose estimation accuracy. Finally, the pose estimation result at the current time T can be determined That is, the weighted sum of the entire particle swarm is calculated according to the weight of each particle:

[0073]

[0074] Example:

[0075] Table 1 Experimental results of 13 objects including FMB benchmark.

[0076]

[0077] like Figure 3 As shown in the figure, the effect comparison shows the pose estimation accuracy of the action selection module (a) and the weight update module (b), where the error value is calculated based on the distance metric Δt+0.1ΔR. Figure 4 As shown in the figure, the effect comparison shows that: From these figures, it can be seen that the method proposed in the present invention has made significant improvements in ensuring rapid convergence of each degree of freedom. (Take the particle position update process as an example).

[0078] 1. Effect comparison

[0079] In order to verify the pose estimation results of the proposed method, the present invention uses a robot equipped with a two-finger gripper to actively grasp the slave object and make contact with the master object, where the robot is a FrankaEmika Panda manipulator equipped with a pair of tactile sensors Tac3D-A1, such as Figure 2 As shown in Figure 2, the maximum change rate of the tactile sensor is 15 mm / s, which means that the robot should move slowly towards the main object. In addition, the present invention uses an RGB-D camera (ORBBEC Femto Bolt) for environmental observation.

[0080] 2. Evaluation and comparison based on FMB benchmark

[0081] Nine different nail hole shapes (150 mm in length) were selected from the FMB benchmark object model as experimental objects, consisting of different shapes, sizes, and colors. In addition, the present invention also designed four new shapes of the same height, forming a total of 13 different object shapes.

[0082] Based on the weight of particle update, the present invention predicts the pose The final results are shown in Table 1. This demonstrates that the evaluation experiments conducted on precise models of different objects achieve high pose estimation accuracy, and that this accuracy can be further improved with increasing the number of particles. This improvement is due to the ability of the particle filter algorithm to attenuate the uncertainty of the pose represented by the particles.

[0083] 2. Test experiments of each module

[0084] Action Selection Module: The actions selected in this module include the contact area and interaction count of the primary object. Force-based tactile data is collected and used to calculate contact information between the two objects. To evaluate the effectiveness of the action selection strategy in the proposed method, it was compared with a random action selection method (referred to as 'random' in this paper).

[0085] like Figure 3 As shown in (a), the results demonstrate that the proposed action selection method achieves pose estimation with a minimal number of contacts, compared to the 'random' method. The proposed action selection method rapidly improves performance with increasing interactions, while the 'random' method converges only slowly with increasing contacts. When a certain number of contacts is reached, the object's 6DOF pose can be fully determined.

[0086] Weight Update Module: Some methods simply treat the contact data as binary (referred to as the '0-1' method in this paper). In order to evaluate the effectiveness of the proposed weight update method, the two methods are evaluated in terms of the performance of the particle filter and the pose estimation results.

[0087] Figure 3 (b) shows that, compared with the '0-1' method, the proposed weight update method not only improves the accuracy of pose estimation, but also speeds up the convergence of the particle filter to the true value in the Bayesian framework. In fact, the weight update module is one of the more time-consuming steps in the algorithm.

[0088] Visual update module: Regarding the evaluation of the impact of visual information on tactile information, the present invention replaces the initial pose estimation of the FoundationPose method with a real value with a large amount of noise added.

[0089] The results are as follows Figure 4 As shown in Figure 3, it shows that the method of the present invention iteratively corrects the weights of the particles through interactive perception. When the true value with a lot of noise is regarded as the initial pose instead of the result estimated by the FoundationPose method, the computational time cost of the particle filter increases significantly, which may lead to failure to converge to the true value due to insufficient number of particles or iterations.

[0090] Hand-object pose change estimation module: In order to evaluate the effectiveness of the proposed hand-object change estimation, in the experiment, the present invention artificially interferes with the grasped object to simulate the sliding process, and the module is evaluated in the FMB benchmark described above.

[0091] As shown in the experimental results in Table 1, the robot can still perform relatively accurate pose estimation despite the possibility of object slippage. Based on the hand-based object pose change estimation module, the present invention can accurately estimate the object's pose change (translation error is 1-2 mm, and rotation error is approximately 2 degrees).

[0092] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, 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 high-precision object pose estimation and fine manipulation method based on visual-tactile fusion, characterized in that: include: Step 1: Define the object held by the robot end effector as the "slave object" and the object being touched as the "master object"; Estimate the initial poses of the master and slave objects respectively, and track the pose changes of the master object in real time. Then, transfer the pose estimation error of the slave object relative to the master object to the pose estimation error of the slave object relative to the robot end-effector, thereby obtaining the corresponding robot end-effector pose. Step 2: When estimating the position of the slave object relative to the robot end effector, consider that the slave object may slide in the robot end effector and estimate the position change T of the slave object in the robot end effector coordinate system. t s ; Step 3: Use particle filtering technology to obtain accurate pose estimation of the object relative to the robot's end effector through multiple interactions between the robot and the environment. Specifically, Step 31: First use the FoundationPose method to estimate and track the pose x of the object in real time. est , sampling is performed in a uniform distribution within the set range to obtain the initial particle swarm; Step 32: For the jth particle in the current particle swarm, update the posture as follows: Where ν is the systematic error; is the posture of the jth particle at time T-1; It is the posture at time T obtained by updating the posture at time T-1; Step 33: Add small noise to the particles of this round and record the position of the robot end effector relative to the world coordinate system at the current moment. and the pose of the main object relative to the world coordinate system 3D space model of master object and slave object m and O s , and input them into the collision simulation experiment to calculate the particle likelihood value size: in, represents the distance between the master and slave objects calculated from the collision simulation experiment of the jth particle, d maxth and d minth The maximum and minimum distances between the master and slave objects are respectively set; y T Indicates the state observation model result, representing the contact degree between the master object and the slave object, Represents the migration process from the situation of robot and environment contact to simulation, e c is the error caused by contact action; x T Represents the posture vector of the particle swarm; Step 34: normalize the likelihood value of each particle and use it as the weight of the particle; Step 35: Sample each particle according to the weight distribution of the current particle swarm to generate a new particle swarm; if the iterative update stop condition is not met, return to step 32 to continue updating the particle swarm; if the iterative update stop condition is met, perform weighted summation on the postures of all particles in the current particle swarm based on the weights to obtain the final output of the slave object posture.

2. The high-precision object pose estimation and fine manipulation method based on visual-tactile fusion according to claim 1, characterized in that: In step 1, the method for obtaining the corresponding robot end effector posture includes: First, use the FoundationPose method to estimate the initial pose results of the master object and the slave object respectively and Then the true values ​​of the two poses are expressed as where e m and e s Represent the errors of the pose estimation of the master object and the slave object respectively; then, define the relative pose of the slave object with respect to the master object And the two error terms e m and e s Integrate into an overall error term e o In the total error term e o Refers to the overall error in the pose estimation from the object and the main object, and the formula is as follows: In the above formula, the pose of the object in the world coordinate system By obtaining the pose of the object relative to the robot's end effector and the pose of the end effector in the world coordinate system To estimate; from the pose of the object relative to the main object Further written as After using tactile information for pose correction, based on the path points of the slave object in the master object coordinate system The corresponding robot end effector pose can be obtained as follows:

3. A high-precision object pose estimation and fine manipulation method based on visual-tactile fusion according to claim 1 or 2, characterized in that: In step 2, the pose change T of the object in the robot end effector coordinate system is estimated. t s The methods include: First, a smoothing filter is applied to the raw data to remove edges and some noise points, and then the feature points around the contact surface are calculated; Based on the calculation and extraction of feature points, the real-time displacement of the center of the contact area is calculated according to the features, and finally the translation slip T of the object in the robot end effector coordinate system at this moment is calculated. tran =||vw||2, where v and w represent the centers of the two contact areas before and after sliding, respectively; The rotational slip T is inferred from the uneven distribution of the marker rotation angle along the rotation center. rota ; Finally, the hand object pose change estimate T t s By translation slip T tran and rotational slip T rota When the sliding is terminated, the calculation of its pose change is stopped.

4. The high-precision object pose estimation and fine manipulation method based on visual-tactile fusion according to claim 3, characterized in that: The rotational slip is given by The calculation shows that the atan(y,x) function returns the angle between the line connecting the point (x,y) and the origin (0,0) and the positive direction of the x-axis, which can be used to obtain the corresponding rotational posture change; where n represents the number of sampled markers, and (xn,yn) represents the coordinates of marker point n relative to the sensor origin.

Images