High-precision object pose estimation and fine operation method based on vision-touch fusion

Abstract

The invention provides a high-precision object pose estimation and fine operation method based on visual and tactile fusion, and the method comprises the steps: obtaining visual information and tactile information through the physical interaction of a robot and an environment, thereby completing the precise pose estimation in a dynamic scene; according to the method, a hand object pose correction algorithm based on particle filtering is constructed, the pose variation when a held object slides and when a contacted object moves can be estimated, and the method is suitable for most high-precision operation scenes, including challenging scenes with dynamic attribute changes, and can be applied to the field of high-precision operation. The coping capacity of the robot in a dynamic interaction environment is obviously improved; according to the method, high-precision operation tasks are executed on 13 objects with different geometrical shapes including the FMB reference, the result shows that the method achieves accurate pose estimation, the translation error is 1-2 mm, the rotation error is about 2 degrees, and meanwhile the method 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 posture estimation and fine operation method based on vision-tactile fusion. Background Art

[0002] Traditional methods of object pose estimation mainly rely on visual information, perform feature extraction and matching based on image preprocessing results, and finally obtain the object pose estimation result based on the matched feature points. In recent years, research on tactile sensors has made significant progress, which can serve as a supplement to vision and enhance the accuracy of pose estimation. Combining the two natural perception methods of vision and touch, the strategies for high-precision pose estimation can be mainly divided into two categories. A common strategy is to use multimodal data fusion to extract information such as point clouds from tactile data, and use optimization algorithms or neural networks to fuse it with visual data to obtain a compact and efficient multimodal representation, thereby enhancing the accuracy of pose estimation. Another research method is to first use visual information to make a preliminary pose estimate, and then use particle filtering and other technologies combined with tactile data for fine-tuning and optimization to achieve high-precision pose estimation.

[0003] Due to factors such as noise and object occlusion, the accuracy of object pose estimation obtained by methods that rely solely on visual information is low. Touch can be used as a complementary modality to vision, and higher-precision pose estimation can be performed based on the two natural modalities of vision and touch. However, most of these visual-tactile methods assume that the position of the held object remains unchanged during robot operation, or that the contacted object remains absolutely still. They lack the ability to adapt to environmental changes, such as being unable to cope with situations where related objects slide or move. Some studies consider object sliding, first using visual information to obtain rough pose results, and then using tactile information to adjust them. However, they usually only detect the initial sliding or adjust the clamping force based on the initial sliding, ignoring the prediction of the change in the object's pose after sliding. These research methods are not applicable in the dynamic operation 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 object and the slave object respectively, and track the pose change process 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, so as to obtain the corresponding pose of the robot end effector.

[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, including:

[0010] Step 31: Use the FoundationPose method to estimate and track the pose x of the object in real time. est , sample in a uniform distribution within its set range to obtain an initial particle group;

[0011] Step 32: For the jth particle in the current particle swarm, update the posture according to the following formula:

[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 the simulation, e c is the error caused by contact action; x T Represents the attitude vector of the particle swarm;

[0017] Step 34: normalize the likelihood value of each particle 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 based on the weights for the postures of all particles in the current particle swarm 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 main 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 relative to the master object And the two error terms e m and e s Integrate into an overall error term e o In the equation, the total error term e o It refers to the overall error of 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, from the position of the object in the world coordinate system By obtaining the pose of the object relative to the robot end effector and the pose of the end effector in the world coordinate system To estimate; from the object's position 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 position can be obtained as:

[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 the 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 angles 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 stops, the calculation of its pose change is stopped.

[0030] Preferably, the rotational slip is It is calculated that the atan(y,x) function returns the angle between the line between the point (x,y) and the origin (0,0) and the positive direction of the x-axis, and the corresponding rotation posture change can be obtained; where n represents the number of sampled marker points, 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 information and tactile information through 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 (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 about 2 degrees, and can cope with dynamic situations such as object sliding. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 It is a schematic diagram of the method framework of the present invention;

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

[0035] Figure 3 To compare the effects: 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;

[0036] Figure 4 For effect comparison: It can be seen from these figures that the method proposed by the present invention has made significant improvements in ensuring rapid convergence of each degree of freedom (taking the updating process of particle position 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 the solution of the present invention has the ability to autonomously grasp an object, and can operate autonomously, using the object to contact another object in the environment. In order to improve applicability, the object held by the robot 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, the meaning of each parameter or symbol is explained first: 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, robot kinematics, known object 3D model and RGB-D information, the method of the present invention aims to estimate the precise 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 scheme of the present invention, tactile measurement refers to the recording of tactile information related to 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 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 with 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 Integrate 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, from the position of the object in the world coordinate system The position 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 in the master object coordinate system is calculated based on the path points of the slave object in the master object coordinate system. (p refers to the path point), the corresponding robot end effector position 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, due to the smooth surface of the tactile sensor or frequent object sliding caused by the collision between the robot and the environment, the gripping uncertainty in the robot movement may be caused. Therefore, after the present invention integrates the estimated error of the posture change of the master object and the slave object into the posture estimation of the slave object relative to the robot end effector, it is necessary to estimate the posture change of the slave object when it slides on the robot end effector, and then enter the particle filtering 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 the relative displacement of the sensor and the object surface in the peripheral area, while the central area remains adhered to the contact surface until the full sliding occurs. Therefore, the 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 the edges and some noise points, and then calculates the feature points around the contact surface. Based on the calculation and extraction of the feature points, the present invention calculates the real-time displacement of the center of the contact area according to 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 derived from 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 stops, 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 errors caused by the system error and the contact action respectively. The position of the robot end effector relative to the world coordinate system at time t is x t It represents the position represented by each particle at time t in the particle filter (i.e., the state in the state space model: the position of the hand object), and 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 where the robot and the environment are in contact 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 object and the slave object in the hand object pose estimation, the present invention uses particle filtering to generate and maintain a set of potential candidate hypotheses about the hand object 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 within a reasonable range to initialize 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 at which weights are updated in the particle filter.

[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 weight of the particle, and then the next iterative update is performed. 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] Wherein, equation (7) represents the system model, equation (8) is the new particle swarm obtained at time T-1 by sampling, and N represents that the particle swarm has a total of N particles.

[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 the three-dimensional force that reaches 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 object collision, some degrees of freedom can be determined for each contact, which will help select the contact area with the most information, and then the next contact can 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 assumption. 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 transformation 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 allocate 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 between the master object and the slave object. minth The particles with a distance of 1 indicate that the two objects are in contact and are assigned a value of 1. maxth The particles with the minimum threshold d 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 comparison of the effects 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 by the present invention has made significant improvements in ensuring the rapid convergence of each degree of freedom. (Take the updating process of particle position 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. 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 comparison based on FMB benchmark

[0081] Nine different nail hole shapes (length of 150 mm) were selected from the object model of the FMB benchmark as experimental objects, which consisted 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 object sets of different shapes.

[0082] Based on the weight of particle update, the present invention predicts the pose The final results are shown in Table 1. It shows that the evaluation experiments conducted on accurate models of different objects have high pose estimation accuracy, and the accuracy can be further improved as the number of particles increases. This improvement is due to the ability of the particle filter algorithm, which comes from the attenuation of 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 the number of interactions of the main object. The present invention collects force-based tactile data, which is used to calculate the contact information of the two objects. In order to evaluate the effectiveness of the action selection strategy in the proposed method, the proposed method is compared with a random action selection method (referred to as 'random' in the present invention).

[0085] like Figure 3 (a) shows that the proposed action selection method can achieve pose estimation with the minimum number of contacts compared to the 'random' method. The performance of the proposed action selection method improves rapidly with the increase of the number of interactions, while the 'random' method can only converge slowly with the increase of the number of contacts. When a certain number of contacts is reached, the six-degree-of-freedom pose of the object can be completely 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 evaluating 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 4As shown in , 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 result in 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 this module is evaluated in the FMB benchmark described above.

[0091] As shown in the experimental results of Table 1, although the object may slide, the robot can still perform relatively accurate posture estimation. Based on the hand object posture change estimation module, the present invention can accurately estimate the object posture change (translation error is 1-2 mm, rotation error is about 2 degrees).

[0092] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the protection scope of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope 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 that the object held by the robot end effector is called the "slave object", and the object being touched is called the "master object"; Estimate the initial poses of the master object and the slave object respectively, and track the pose change process 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, so as to obtain the corresponding pose of the robot end effector. Step 2: When estimating the position and posture 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 and posture 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, including: Step 31: Use the FoundationPose method to estimate and track the pose x of the object in real time. est , sample in a uniform distribution within its set range to obtain an initial particle group; Step 32: For the jth particle in the current particle swarm, update the posture according to the following formula: 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 the simulation, e c is the error caused by contact action; x T Represents the attitude vector of the particle swarm; Step 34: normalize the likelihood value of each particle 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 based on the weights for the postures of all particles in the current particle swarm 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 as claimed in claim 1, characterized in that: In step 1, the method for obtaining the corresponding robot end effector position and posture includes: First, use the FoundationPose method to estimate the initial pose results of the main 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 relative to the master object And the two error terms e m and e s Integrate into an overall error term e o In the equation, the total error term e o It refers to the overall error of the pose estimation from the object and the main object, and the formula is as follows: In the above formula, from the position of the object in the world coordinate system By obtaining the pose of the object relative to the robot end effector and the pose of the end effector in the world coordinate system To estimate; from the object's position 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 position can be obtained as:

3. A high-precision object pose estimation and fine manipulation method based on visual-tactile fusion as claimed in 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 the 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 angles 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 stops, the calculation of its pose change is stopped.

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

Images