Method and System for Attitude Estimation of a Mechanical Hand-held Acupuncture Needle Based on Visual and Tactile Sensing

Abstract

The present invention discloses a method and system for attitude estimation of a mechanical hand-held acupuncture needle based on visual and tactile perception. The method includes: using a gripper to grasp the acupuncture needle in the current attitude and the target attitude, and obtaining the tactile images corresponding to the attitudes; processing the tactile images to calculate the angle, length and position of the acupuncture needle in the tactile images of the two different attitudes, and calculating a rough matching matrix from the current attitude to the target attitude; processing the tactile images to generate the tactile point cloud corresponding to the attitude, then performing 2D attitude alignment on the tactile point cloud in the current attitude based on the rough matching matrix, and then calculating a fine transformation matrix based on the 2D-aligned tactile point cloud and the tactile point cloud in the target attitude by using the ICP point cloud registration method; controlling the rotation and translation of the mechanical hand gripper based on the rough matching matrix and the fine transformation matrix. Compared with the attitude estimation using only ICP matching, the present invention has better alignment ability and faster convergence.

Description

Technical Field

[0001] The present invention relates to the technical field of robot tactile perception, and particularly to a method and system for estimating the posture of a mechanical hand holding an acupuncture needle based on visual-tactile perception. Background Art

[0002] With the continuous innovation and development of traditional Chinese medicine, the acceptance of acupuncture by the majority of patients is getting higher and higher. However, the learning of traditional Chinese medicine acupuncture techniques is a long process that requires doctors to accumulate years of learning and experience. With the rapid progress of modern science and technology, many researchers have begun to use robots to learn acupuncture techniques, hoping to accelerate the learning and implementation of traditional Chinese medicine acupuncture techniques through current machine learning or deep learning methods. However, these operations need to be completed autonomously or semi-autonomously by the robot. In this process, the determination of the posture of the robot holding the acupuncture needle is particularly important, which is the first step in all acupuncture technique learning and implementation by the robot. Only when the robot knows the posture of the acupuncture needle in its "hand" can it accurately perform acupuncture techniques such as lifting and thrusting, twirling and rotating. Summary of the Invention

[0003] The present invention provides a method and system for estimating the posture of a mechanical hand holding an acupuncture needle based on visual-tactile perception, with good alignment ability and fast convergence.

[0004] To achieve the above technical objectives, the present invention adopts the following technical solutions:

[0005] A method for estimating the posture of a mechanical hand holding an acupuncture needle based on visual-tactile perception, comprising:

[0006] Using a robotic gripper equipped with a visual-tactile sensor to grasp the acupuncture needle in two different postures, defining the two different postures as the current posture and the target posture respectively, and obtaining the tactile image in the current posture and the tactile image in the target posture ;

[0007] Performing first processing on the tactile images and respectively to calculate the angles, lengths, and positions of the acupuncture needle in the tactile images of the two different postures, and calculating a rough matching matrix from the current posture to the target posture based on the obtained angles, lengths, and positions ;

[0008] Based on the rough matching matrix controlling the robotic gripper to rotate and translate in the 2D plane;

[0009] Performing second processing on the tactile images and respectively to generate the tactile point cloud in the current posture and the tactile point cloud in the target pose ; then based on the rough matching matrix perform 2D pose alignment on the tactile point cloud in the current pose to obtain the roughly matched tactile point cloud in the current pose ; then based on the tactile point cloud and and adopt the ICP point cloud registration method to calculate the fine transformation matrix between the current tactile point cloud after 2D pose alignment and the tactile point cloud in the target pose ;

[0010] Based on the fine transformation matrix control the manipulator gripper to rotate and translate in 3D space.

[0011] Furthermore, perform a first processing on the tactile image, including: first detecting the contact area of the acupuncture needle on the tactile image, then finding and splicing the contours of the obtained acupuncture needle contact area to obtain the acupuncture needle contour, then finding the minimum bounding matrix of the acupuncture needle contour, and then estimating the angle, length, and position of the acupuncture needle based on the minimum bounding matrix.

[0012] Furthermore, adopt any one of the following operation methods to detect the contact area of the acupuncture needle on the tactile image:

[0013] (1) Perform image smoothing, edge detection, and region filling through image detection technology to achieve contact area detection;

[0014] (2) Generate an acupuncture needle region map through a pre-trained image segmentation neural network model to achieve contact area detection.

[0015] Furthermore, the method for calculating the rough matching matrix based on the obtained angle, length, and position is:

[0016] ;

[0017] ;

[0018] ;

[0019] In the formula, and respectively represent the operations of calculating the translation transformation matrix and the rotation transformation matrix in the robot coordinate transformation, respectively represent the angle, length, and position of the acupuncture needle in the tactile image in the current pose , respectively represent the angle, length, and position of the acupuncture needle in the tactile image in the target pose , , respectively represent half of the angular difference and length difference of the acupuncture needles on two images; and are two translation vectors. First, move the origin of the coordinate system to the center position of the current acupuncture needle area through the translation vector , then achieve rotation through the angular difference to align the target in terms of angle, and finally move the current acupuncture needle to the target position through the translation vector ; The z in

[0020] represents that the rotation transformation matrix rotates around the z-axis.

[0021] Further, during the first processing of the tactile image, if it is found that the handle end of the needle is not included in the image, the gripper is controlled to re-grasp the acupuncture needle so that the handle end of the needle is included in the tactile image, and the first processing and the second processing are updated using the tactile image including the handle end of the needle.

[0022] First, establish a lookup table through photometric stereo method or, through the method of neural network, take the pixel intensity or the pixel intensity and position as input to obtain the gradient angle of the pixel point ( ), and construct the corresponding gradient map ;

[0023] Then, through a two-dimensional fast Poisson solver, solve the corresponding depth map from the gradient map :

[0024] ;

[0025] Among them, is the solution error. The smaller the error, the closer the solved depth map is to the true depth.

[0026] Finally, compare the depth values of each pixel point in the depth map with a preset significance threshold, filter out the pixel points with depths lower than the significance threshold in the depth map, and obtain the final significant tactile point cloud.

[0027] Further, perform 2D pose alignment on the tactile point cloud in the current pose based on the rough matching matrix , which is expressed as: .

[0028] Further, based on the tactile point cloud and And the ICP point cloud registration method is adopted to calculate the fine transformation matrix , which specifically includes:

[0029] (1) Use KD-tree for nearest neighbor search to select paired point cloud sets from the transformed current tactile point cloud and the target tactile point cloud , which are respectively denoted as and , and calculate their respective centroids:

[0030] ;

[0031] In the formula, represents the number of paired point clouds in the current tactile point cloud and the target tactile point cloud , and respectively represent the in the current tactile point cloud and the in the target tactile point cloud that are paired with each other point clouds; and are respectively the centroids of the current tactile point cloud and the target tactile point cloud ;

[0032] (2) Construct the covariance matrix and of the paired point cloud sets , and perform SVD decomposition on the covariance matrix :

[0033] ;

[0034] ;

[0035] In the formula, , , are the three matrices obtained by decomposing , where is a diagonal matrix, and the values on its diagonal are singular values; , are both orthogonal matrices;

[0036] (3) Calculate the rotation matrix and the translation vector :

[0037] ;

[0038] ;

[0039] Among them, the rotation matrix and the translation vector constitute the fine transformation matrix ;

[0040] (4) Calculate the transformed current tactile point cloud and the distance error between the target tactile point cloud :

[0041] ;

[0042] Among them, represents the serial number of the paired point cloud, and represent the th paired point cloud;

[0043] (5) Repeat steps (1) to (4) until the maximum number of iterations is reached or the distance error between the transformed current tactile point cloud and the target tactile point cloud is small enough, then stop the iteration.

[0044] A mechanical hand-held acupuncture needle attitude estimation system based on visual and tactile perception, comprising:

[0045] An image acquisition module, configured to: use a manipulator gripper equipped with a visual and tactile sensor to grasp the acupuncture needle in two different postures, define the two different postures as the current posture and the target posture respectively, and obtain the tactile image in the current posture and the tactile image

[0046] in the target posture; A rough matching module, configured to: perform a first processing on the tactile images and respectively, to calculate the angle, length and position of the acupuncture needle in the tactile images of the two different postures, and calculate the rough matching matrix

[0047] from the current posture to the target posture based on the obtained angle, length and position; An execution module, configured to: control the manipulator gripper to rotate and translate in the 2D plane based on the rough matching matrix

[0048] A fine transformation module, configured to: perform a second processing on the tactile images and respectively, to generate the tactile point cloud in the current posture and the tactile point cloud For the tactile point cloud in the current pose perform 2D pose alignment to obtain the coarsely matched tactile point cloud in the current pose ; then, based on the tactile point cloud and and using the ICP point cloud registration method, calculate the fine transformation matrix between the current tactile point cloud after 2D pose alignment and the target pose tactile point cloud ;

[0049] The execution module is further configured to: based on the fine transformation matrix control the manipulator gripper to rotate and translate in 3D space.

[0050] Compared with the prior art, the beneficial effects of the present invention are:

[0051] Due to the existence of a large number of non-unique contacts in the acupuncture needle itself, simply using point cloud registration is likely to cause obvious errors in the alignment of the pose estimation result of the acupuncture needle in the direction along the needle body. The present invention overcomes problems such as the failure of feature point matching, low point cloud registration accuracy or failure caused by the high local similarity between the tactile texture image and the tactile point cloud of the acupuncture needle, and has better alignment ability and faster convergence compared with the robot-held acupuncture needle pose estimation using only ICP matching. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 is a flowchart of the method for estimating the 3D pose of a hand-held acupuncture needle in the present invention.

[0053] Figure 2 is the pose estimation process of the embodiment of the present invention.

[0054] Figure 3 is a display diagram of the gripper grasping the acupuncture needle in the present invention.

[0055] Figure 4 is a comparison of the pose estimation result of the embodiment of the present invention with the result using only ICP matching, where (a) is the pose estimation result of the embodiment of the present invention and (b) is the result using only ICP matching. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0056] The embodiments of the present invention will be described in detail below. Based on the technical solution of the present invention, detailed implementation manners and specific operation processes are given, and the technical solution of the present invention is further explained.

[0057] This embodiment provides a method for estimating the pose of a mechanically held acupuncture needle based on visual and tactile perception, as shown in Figure 1 、 2 and includes:

[0058] Step 1: Use a robotic gripper with a GelSight tactile sensor to grasp the acupuncture needle in two different postures. As shown in Figure 3 , define the two different postures as the current posture and the target posture respectively, and obtain the tactile images in the current posture and the tactile images in the target posture .

[0059] Step 2: Perform a first processing on the tactile images and respectively to calculate the angles, lengths, and positions of the acupuncture needle in the tactile images of the two different postures, and calculate a rough matching matrix from the current posture to the target posture based on the obtained angles, lengths, and positions .

[0060] The first processing of the tactile images includes:

[0061] Step 2.1: Detect the contact area of the acupuncture needle on the tactile image. Any of the following operation methods can be used to detect the contact area of the acupuncture needle on the tactile image:

[0062] (1) Perform image smoothing, edge detection, and region filling through image detection technology to achieve contact area detection;

[0063] ;

[0064] ;

[0065] ;

[0066] Among them, represents Gaussian filtering, is the tactile image, is the standard deviation, is the image obtained by Gaussian filtering and smoothing; represents Canny edge detection, are the minimum and maximum threshold parameters involved in the edge detection algorithm, is the result of edge detection; represents the image closing operation, is the structuring element, is the number of iterations of the operation, represents the contact area.

[0067] (2) Generate an acupuncture needle region map through a pre-trained image segmentation neural network model to achieve contact area detection:

[0068] ;

[0069] Among them, Represents a pre-trained image segmentation network. The image segmentation neural network model of this embodiment is derived from the recently proposed SAM model (Segment Anything Model).

[0070] Step 2.2: Perform contour search and splicing on the obtained acupuncture needle contact area to obtain the acupuncture needle contour:

[0071] ;

[0072] ;

[0073] Among them, Represents the contour search algorithm, Represents contour splicing.

[0074] Step 2.3: Search for the minimum bounding rectangle of the acupuncture needle contour, and estimate the angle, length, and position of the acupuncture needle based on the minimum bounding rectangle:

[0075] ;

[0076] Among them, Represents the minimum bounding rectangle estimation algorithm, Contains the angle of the minimum bounding rectangle , length And the center point position .

[0077] In addition, the method for calculating the rough matching matrix Based on the obtained angle, length, and position is:

[0078] ;

[0079] ;

[0080] ;

[0081] In the formula, And Respectively represent the operations of calculating the translation transformation matrix and the rotation transformation matrix in the robot coordinate transformation, Respectively represent the angle, length, and position of the acupuncture needle in the current pose tactile image , Respectively represent the angle, length, and position of the acupuncture needle in the target pose tactile image , , Respectively represent half of the angle difference and length difference of the acupuncture needle on the two images; And are two translation vectors. First, move the origin of the coordinate system to the center position of the current acupuncture needle area through the translation vector , then achieve rotation through the angular difference to align the target in terms of angle. Finally, move the current acupuncture needle to the target position through the translation vector ; The z in

[0082] means that the rotation transformation matrix rotates around the z-axis. Since the length of the acupuncture needle is much greater than the length and width of the surface of the visual tactile sensor, when the acupuncture needle crosses the surface of the visual tactile sensor, that is, the end of the needle handle is not included in the tactile image, the gripper can be controlled to move backward a certain distance, re-grasp the acupuncture needle, so that the end of the needle handle is included in the tactile image, and update the first processing using the tactile image including the end of the needle handle.

[0083] Step 3, based on the rough matching matrix control the manipulator gripper to rotate and translate in the 2D plane.

[0084] Step 4, perform a second processing on the tactile images and respectively to generate the tactile point cloud in the current pose and the tactile point cloud in the target pose; then, based on the rough matching matrix perform 2D pose alignment on the tactile point cloud in the current pose to obtain the roughly matched tactile point cloud in the current pose; then, based on the tactile point clouds and and using the ICP point cloud registration method, calculate the fine transformation matrix of the current tactile point cloud after 2D pose alignment and the target pose tactile point cloud.

[0085] Among them, the specific process of performing a second processing on the tactile image to generate a tactile point cloud includes:

[0086] First, establish a lookup table through photometric stereo method or, through a neural network method, use the pixel point intensity or the pixel point intensity and position as inputs to obtain the gradient angle of the pixel point( ), and construct the corresponding gradient map ;

[0087] Then, through a two-dimensional fast Poisson solver, solve the corresponding depth map from the gradient map:

[0088] ;

[0089] Among them, is the solution error. The smaller the error, the closer the solved depth map is to the true depth.

[0090] Finally, the depth values of each pixel point in the depth map are compared with a preset significant threshold, and the pixel points in the depth map with a depth lower than the significant threshold are filtered out to obtain the final significant tactile point cloud. In this embodiment, the point cloud with a depth exceeding 1 / 4 of the maximum depth is judged as the significant point cloud.

[0091] Furthermore, based on the rough matching matrix the tactile point cloud at the current pose is aligned in 2D, expressed as:

[0092] .

[0093] Furthermore, based on the tactile point cloud and and using the ICP point cloud registration method, the fine transformation matrix is calculated, specifically including:

[0094] (1) Use KD-tree for nearest neighbor search to select paired point cloud sets from the transformed current tactile point cloud and the target tactile point cloud , which are respectively expressed as and , and calculate their respective centroids:

[0095] ;

[0096] In the formula, represents the number of paired point clouds in the current tactile point cloud and the target tactile point cloud , and respectively represent the paired point clouds in the current tactile point cloud and the target tactile point cloud; and are the centroids of the current tactile point cloud and the target tactile point cloud respectively.

[0097] (2) Construct the covariance matrix and of the paired point cloud sets, and perform SVD decomposition on the covariance matrix :

[0098] ​ ;

[0099] ;

[0100] In the formula, , , are three matrices obtained by decomposing , where is a diagonal matrix, and the values on its diagonal are singular values; , are both orthogonal matrices.

[0101] (3) Calculate the rotation matrix and the translation vector :

[0102] ;

[0103] ;

[0104] Among them, the rotation matrix and the translation vector constitute the fine transformation matrix .

[0105] (4) Calculate the distance error between the transformed current tactile point cloud and the target tactile point cloud :

[0106] ;

[0107] Among them, represents the serial number of the paired point cloud, and represent the paired point cloud.

[0108] (5) Repeat steps (1) to (4) until the maximum number of iterations is reached or the distance error between the transformed current tactile point cloud and the target tactile point cloud is small enough, then stop the iteration.

[0109] The distance error threshold in this embodiment is 10 pixel distances, and the maximum number of iterations is 500.

[0110] Step 5, based on the fine transformation matrix control the manipulator gripper to rotate and translate in 3D space to complete the alignment from the current pose to the target pose.

[0111] To facilitate the understanding of the technical effects of the present invention, a comparison between the present invention and the case of only using ICP matching is provided for reference. Figure 4 It can be seen that when directly performing ICP matching on the target tactile point cloud and the current tactile point cloud of the acupuncture needle, there will be a position deviation in the direction along the needle body in the matching result. The method of the present invention greatly reduces this error.

[0112] The above embodiments are the preferred embodiments of the present application. Those of ordinary skill in the art can also make various transformations or improvements based on this. Without departing from the general concept of the present application, these transformations or improvements should fall within the scope of protection required by the present application.

Claims

1. A method for estimating the posture of a mechanical hand-held acupuncture needle based on visual and tactile perception, characterized in that Including: Grasp the acupuncture needle in two different postures using a robotic manipulator gripper equipped with a visual-tactile sensor, define the two different postures as the current posture and the target posture respectively, and obtain the tactile image in the current posture and the tactile image in the target posture ; Process the tactile images separately and Perform a first processing to calculate the angles, lengths, and positions of the acupuncture needles in the tactile images of two different poses, and calculate a rough matching matrix from the current pose to the target pose based on the obtained angles, lengths, and positions ; Based on the rough matching matrix Control the rotation and translation of the robotic manipulator gripper in a 2D plane; The haptic images are respectively and subjected to a second process to generate the haptic point cloud in the current pose and the haptic point cloud in the target pose ; Then, based on the rough matching matrix perform 2D pose alignment on the tactile point cloud at the current pose to obtain the roughly matched tactile point cloud at the current pose ; Then, based on the tactile point cloud and and using the ICP point cloud registration method, calculate the fine transformation matrix between the current tactile point cloud after 2D pose alignment and the target pose tactile point cloud ; Based on a fine transformation matrix Control the rotation and translation of the robotic manipulator gripper in 3D space.

2. The method for estimating the posture of a mechanical hand-held acupuncture needle according to claim 1, characterized in that, Performing a first processing on the tactile image, including: first detecting the contact area of the acupuncture needle on the tactile image, then finding and splicing the contours of the obtained acupuncture needle contact area to obtain the acupuncture needle contour, then finding the minimum bounding rectangle of the acupuncture needle contour, and then estimating the angle, length, and position of the acupuncture needle based on the minimum bounding rectangle.

3. The method for estimating the pose of the mechanical hand-held acupuncture needle according to claim 2, wherein, Detecting the contact area of the acupuncture needle on the tactile image by using any one of the following operation methods: (1) Performing image smoothing, edge detection, and region filling through image detection technology to achieve contact area detection; (2) Generating an acupuncture needle region map through a pre-trained image segmentation neural network model to achieve contact area detection.

4. The attitude estimation method of the mechanical hand-held acupuncture needle according to claim 1, characterized in that, Calculate a rough matching matrix based on the obtained angles, lengths, and positions The method is as follows: ； ； ； Wherein, and respectively represent the operations of calculating the translation transformation matrix and the rotation transformation matrix in the robot coordinate transformation, respectively represent the current posture tactile image the angle, length and position of the acupuncture needle in, respectively represent the target posture tactile image the angle, length and position of the acupuncture needle in, , respectively represent half of the angle difference and length difference of the acupuncture needles on the two images; and are two translation vectors. First, the origin of the coordinate system is moved to the center position of the current acupuncture needle area through the translation vector , and then rotation is achieved through the angle difference to align the target in terms of angle. Finally, the current acupuncture needle is moved to the target position through the translation vector ; the z in represents that the rotation transformation matrix rotates around the z-axis.

5. The posture estimation method of the mechanical hand-held acupuncture needle according to claim 1, characterized in that, If it is found during the first processing of the tactile image that the tactile image does not include the end of the needle handle, then control the gripper to re-grasp the acupuncture needle so that the tactile image includes the end of the needle handle, and update the use of the tactile image including the end of the needle handle for the first processing and the second processing.

6. The method for estimating the posture of the mechanical hand-held acupuncture needle according to claim 1, characterized in that The specific process of performing a second processing on the tactile image to generate a tactile point cloud includes: First, a lookup table is established through photometric stereo, or through a neural network method, to obtain the intensity of the pixel points of the tactile image or the intensity and position of the pixel points as inputs, and the gradient angle of the pixel points ( ) is obtained to construct the corresponding gradient map ; Then, a corresponding depth map is solved from the gradient map by a two-dimensional fast Poisson solver : ； Among them, is the solution error. The smaller the error, the closer the solved depth map is to the true depth; Finally, compare the depth values of each pixel in the depth map with a preset significant threshold, filter out the pixels in the depth map with a depth lower than the significant threshold, and obtain the final significant tactile point cloud.

7. The method for estimating the posture of a mechanical hand-held acupuncture needle according to claim 1, characterized in that, Based on the rough matching matrix Perform 2D pose alignment on the tactile point cloud in the current pose, expressed as: . .

8. The method for estimating the posture of the mechanical hand-held acupuncture needle according to claim 1, characterized in that, Based on tactile point cloud and and adopt the ICP point cloud registration method to calculate the fine transformation matrix , specifically including: (1) Use the KD-tree for nearest neighbor search to select paired point cloud sets from the transformed current haptic point cloud and the target haptic point cloud respectively, which are denoted as and , and calculate their respective centroids: ； In the formula, represents the current tactile point cloud and the target tactile point cloud is the number of paired point clouds in and respectively represent the current tactile point cloud and the target tactile point cloud in which the mutually paired point clouds; and are respectively the centroids of the current tactile point cloud and the target tactile point cloud ; (2)Construct a set of corresponding point clouds and covariance matrix , and perform SVD decomposition on the covariance matrix : ； ； In the formula, , , are three matrices obtained by decomposing , where is a diagonal matrix, and the values on its diagonal are singular values; , are both orthogonal matrices. (3) Calculate the rotation matrix and the translation vector : ； ； Among them, the rotation matrix and the translation vector constitute the fine transformation matrix ; (4) Calculate the transformed current tactile point cloud and the target tactile point cloud the distance error between : ； Among them, represents the serial number of the paired point cloud, and represents the paired point cloud; (5) Repeat steps (1) to (4) until the maximum number of iterations is reached or the distance error between the current transformed haptic point cloud and the target haptic point cloud is small enough, then stop the iteration. ​ 9. An attitude estimation system for a mechanical hand-held acupuncture needle based on visual and tactile perception, characterized in that, Including: An image acquisition module, configured to: use a manipulator gripper equipped with a visual-haptic sensor to grasp an acupuncture needle in two different postures, define the two different postures as the current posture and the target posture respectively, and acquire a haptic image in the current posture and a haptic image in the target posture ; A rough matching module for: respectively performing a first processing on the tactile images and to calculate the angles, lengths, and positions of the acupuncture needles in the tactile images of two different poses respectively, and calculating a rough matching matrix from the current pose to the target pose based on the obtained angles, lengths, and positions ; Execution module, configured to: based on the rough matching matrix control the manipulator gripper to rotate and translate within a 2D plane; A fine transformation module for: separately performing a second process on the tactile image and to generate a tactile point cloud in the current pose and a tactile point cloud in the target pose ; Then, based on the rough matching matrix perform 2D pose alignment on the tactile point cloud at the current pose to obtain the roughly matched tactile point cloud at the current pose ; Then, based on the tactile point cloud and and using the ICP point cloud registration method, calculate the fine transformation matrix between the current tactile point cloud after 2D pose alignment and the target pose tactile point cloud ; The execution module is further configured to: based on the fine transformation matrix control the manipulator gripper to rotate and translate in 3D space.

Images