Visual tactile perception-based attitude estimation method and system for mechanical handheld acupuncture needle

Abstract

The invention discloses an attitude estimation method and system for a mechanical handheld acupuncture needle based on visual tactile perception, and the method comprises the steps: grabbing the acupuncture needle under a current attitude and a target attitude through a clamping device, and obtaining a tactile image under a corresponding attitude; the tactile images are processed to calculate the angle, length and position of the acupuncture needle in the tactile images of the two different postures, and a rough matching matrix from the current posture to the target posture is calculated; processing the tactile image to generate a tactile point cloud under a corresponding attitude, then performing 2D attitude alignment on the tactile point cloud under the current attitude based on the rough matching matrix, and then calculating a fine transformation matrix based on the tactile point cloud after 2D alignment and the tactile point cloud under the target attitude by adopting an ICP point cloud registration method; and controlling the manipulator holder to rotate and translate based on the rough matching matrix and the fine transformation matrix. Compared with attitude estimation only using ICP matching, the method has better alignment capability 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 and 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, etc. 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 and tactile perception, which has good alignment ability and fast convergence.

[0004] To achieve the above technical objectives, the present invention adopts the following technical solutions: A method for estimating the posture of a mechanical hand holding an acupuncture needle based on visual and tactile perception, comprising: Using a mechanical hand gripper equipped with a visual and 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 ; Performing 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 calculating a rough matching matrix from the current posture to the target posture based on the obtained angle, length and position ; Based on the rough matching matrix controlling the mechanical hand gripper to rotate and translate in the 2D plane; 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 posture ; then based on the rough matching matrix performing on the tactile point cloud in the current posture 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 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 target pose tactile point cloud. ; Based on the fine transformation matrix control the manipulator gripper to rotate and translate in 3D space.

[0005] 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 rectangle of the acupuncture needle contour, and then estimating the angle, length, and position of the acupuncture needle based on the minimum bounding rectangle.

[0006] Furthermore, use any of the following operation methods to detect the contact area of the acupuncture needle on the tactile image: (1) Perform image smoothing, edge detection, and region filling through image detection technology to achieve contact area detection; (2) Generate an acupuncture needle area map through a pre-trained image segmentation neural network model to achieve contact area detection.

[0007] Furthermore, the method for calculating the rough matching matrix based on the obtained angle, length, and position is: ; ; ; 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 coordinate system origin to the center position of the current acupuncture needle area through the translation vector , then achieve rotation through the angle difference to align the target in terms of angle, and finally through the translation vector Move the current acupuncture needle to the target position; In, z represents that the rotation transformation matrix rotates around the z-axis.

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

[0009] Further, the specific process of performing the second processing on the tactile image to generate a tactile point cloud includes: First, establish a look-up table by photometric stereo method or, by means of a neural network, use the pixel intensity or the pixel intensity and position as inputs to obtain the gradient angle of the pixel points ( ), and construct the corresponding gradient map ; Then, use a two-dimensional fast Poisson solver to solve the corresponding depth map from the gradient map : ; Among them, is the solution error. The smaller the error, the closer the solved depth map is to the true depth.

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

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

[0012] Further, based on the tactile point cloud and and using the ICP point cloud registration method, calculate the fine transformation matrix , specifically including: (1) Use KD-tree for nearest neighbor search, and 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: ; In the formula, Represents the current tactile point cloud and the target tactile point cloud The number of paired point clouds in and respectively represent the current tactile point cloud and the target tactile point cloud The mutually paired point clouds in; and are respectively the centroids of the current tactile point cloud and the target tactile point cloud ; (2)Construct the covariance matrix of the paired point cloud set and and perform SVD decomposition on the covariance matrix : ; ; ; 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; (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 distance error between the transformed current tactile point cloud and the target tactile point cloud : ; Among them, represents the serial number of the paired point cloud, and represent the th paired point cloud; (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 ​ Stop iteration when it is small enough.

[0013] A posture estimation system for a mechanical hand-held acupuncture needle based on visual and tactile perception, comprising: An image acquisition module, configured to: use a mechanical hand 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 in the target posture ; A rough matching module, configured to: perform 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 a rough matching matrix from the current posture to the target posture based on the obtained angle, length and position ; An execution module, configured to: control the mechanical hand gripper to rotate and translate in a 2D plane based on the rough matching matrix ; A fine transformation module, configured to: perform second processing on the tactile images and respectively to generate a tactile point cloud in the current posture and a tactile point cloud in the target posture ; then perform 2D posture alignment on the tactile point cloud in the current posture based on the rough matching matrix to obtain a roughly matched tactile point cloud in the current posture ; further calculate a fine transformation matrix of the current tactile point cloud after 2D posture alignment and the tactile point cloud in the target posture based on the tactile point clouds and and by using the ICP point cloud registration method ; The execution module is further configured to: control the mechanical hand gripper to rotate and translate in a 3D space based on the fine transformation matrix ;

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

[0015] Figure 1 This is the flowchart of the method for 3D pose estimation of a handheld acupuncture needle in the present invention.

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

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

[0018] Figure 4 This is the comparison between the pose estimation result of the embodiment of the present invention and the result of only using ICP matching, where (a) is the pose estimation result of the embodiment of the present invention, and (b) is the result of only using ICP matching. Detailed implementation manners

[0019] The following provides a detailed description of the embodiments of the present invention. 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.

[0020] This embodiment provides a method for pose estimation of a mechanical handheld acupuncture needle based on visual and tactile perception, as Figure 1 , 2 shown, including: Step 1: Use a robotic gripper with a GelSight visual and tactile sensor to grasp the acupuncture needle in two different poses, as Figure 3 shown. Define the two different poses as the current pose and the target pose respectively, and obtain the tactile image in the current pose and the tactile image in the target pose .

[0021] Step 2: 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 poses, and calculate the rough matching matrix from the current pose to the target pose based on the obtained angle, length, and position.

[0022] Performing the first processing on the tactile image includes: 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: (1) Perform image smoothing, edge detection, and region filling through image detection technology to achieve contact area detection; ; ; ; 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, is the minimum and maximum threshold parameter involved in the edge detection algorithm, is the result obtained by edge detection; represents the image closing operation, is the structuring element, is the number of iterations of the operation, represents the contact area.

[0023] (2) Through a pre-trained image segmentation neural network model, generate an acupuncture needle area map to achieve contact area detection: ; Among them, represents the pre-trained image segmentation network. The image segmentation neural network model in this embodiment is derived from the recently proposed SAM model (Segment Anything Model).

[0024] Step 2.2, perform contour finding and splicing on the obtained acupuncture needle contact area to obtain the acupuncture needle contour: ; ; Among them, represents the contour finding algorithm, represents contour splicing.

[0025] Step 2.3, find 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: ; Among them, represents the minimum bounding rectangle estimation algorithm, contains the angle of the minimum bounding rectangle , the length and the center point position .

[0026] In addition, the method for calculating the rough matching matrix based on the obtained angle, length and position is: ; ; ; 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 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

[0027] 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 handle end of the needle is not included in the tactile image, the gripper can be controlled to move backward by a certain distance, and the acupuncture needle can be re-gripped to make the handle end of the needle included in the tactile image, and the first processing is updated using the tactile image including the handle end of the needle.

[0028] Step 3: Control the manipulator gripper to rotate and translate in the 2D plane based on the rough matching matrix .

[0029] 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.

[0030] Among them, the specific process of performing a second processing on the tactile image to generate the tactile point cloud includes: First, a lookup table is established through photometric stereo method, or by means of a neural network, and the intensity of pixel points of the tactile image or the intensity and position of pixel points are used as inputs to obtain the gradient angle of pixel points ( ), and a corresponding gradient map is constructed; Then, through a two-dimensional fast Poisson solver, the corresponding depth map is solved from the gradient map : ; Among them, is the solution error. The smaller the error, the closer the solved depth map is to the true depth.

[0031] 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.

[0032] Furthermore, based on the rough matching matrix the tactile point cloud under the current pose is aligned in 2D, which is expressed as: .

[0033] Furthermore, based on the tactile point cloud and and adopting the ICP point cloud registration method, the fine transformation matrix is calculated, which specifically includes: (1) Using KD-tree for nearest neighbor search, from the transformed current tactile point cloud and the target tactile point cloud paired point cloud sets are selected, which are respectively expressed as and , and their centroids are calculated: ; 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 target tactile point cloud paired and are respectively the current tactile point cloud and the centroid of the target haptic point cloud .

[0034] (2)Construct a set of corresponding point clouds and the covariance matrix of , 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.

[0035] (3)Calculate the rotation matrix and the translation vector : ; ; Among them, the rotation matrix and the translation vector constitute the fine transformation matrix .

[0036] (4)Calculate the distance error between the transformed current haptic point cloud and the target haptic point cloud : ; Among them, represents the serial number of the corresponding point cloud, and represent the th corresponding point cloud.

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

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

[0039] 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.

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

[0041] 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 overall 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 handheld acupuncture needle based on visual-tactile perception, characterized in that: include: Use a manipulator gripper equipped with a visual tactile sensor to grasp acupuncture needles in two different postures, define the two different postures as the current posture and the target posture, and obtain the tactile image in the current posture and tactile image of the target posture ; Tactile image and The first processing is performed to calculate the angle, length and position of the acupuncture needle in the two different posture tactile images, and based on the obtained angle, length and position, a rough matching matrix from the current posture to the target posture is calculated. ; Based on the rough matching matrix Control the robot gripper to rotate and translate in a 2D plane; Tactile image and Perform the second processing to generate the tactile point cloud under the current posture And tactile point cloud in target posture ; Then based on the rough matching matrix Tactile point cloud for the current posture Perform 2D posture alignment to obtain a roughly matched tactile point cloud under the current posture ; Based on the tactile point cloud and The ICP point cloud registration method is used to calculate the fine transformation matrix of the current tactile point cloud and the target tactile point cloud after 2D posture alignment. ; Based on the refined transformation matrix Control the robot gripper to rotate and translate in 3D space.

2. The method for estimating the posture of a robot-held acupuncture needle according to claim 1, characterized in that: The tactile image is first processed, including: first detecting the contact area of ​​the acupuncture needle on the tactile image, then performing contour search and splicing on the obtained contact area of ​​the acupuncture needle to obtain the contour of the acupuncture needle, then searching for the minimum circumscribed matrix of the contour of the acupuncture needle, and then estimating the angle, length and position of the acupuncture needle based on the minimum circumscribed matrix.

3. The method for estimating the posture of a robot-held acupuncture needle according to claim 2, characterized in that: Use any of the following operation methods to detect the contact area of ​​the acupuncture needle on the tactile image: (1) Image smoothing, edge detection, and area filling are performed through image detection technology to achieve contact area detection; (2) Generate an acupuncture needle area map through a pre-trained image segmentation neural network model to achieve contact area detection.

4. The method for estimating the posture of a robot-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: ； ； ； In the formula, and They represent the operations of calculating the translation transformation matrix and the rotation transformation matrix in the robot coordinate transformation. Represents the current posture tactile image The angle, length and position of acupuncture needles, Represent the target posture tactile image The angle, length and position of acupuncture needles, , They represent the angle difference and half of the length difference of the acupuncture needles in the two images respectively; and There are two translation vectors. First, we use the translation vector Move the origin of the coordinate system to the center of the current acupuncture needle area, and then use the angle difference Implement rotation, align the target in angle, and finally translate the vector Move the current acupuncture needle to the target position; The z in it means that the rotation transformation matrix rotates around the z axis.

5. The method for estimating the posture of a robot-held acupuncture needle according to claim 1, characterized in that: If it is found during the first processing of the tactile image that the image does not include the needle handle end, the clamp is controlled to re-grasp the acupuncture needle so that the tactile image includes the needle handle end, and the tactile image including the needle handle end is updated to perform the first processing and the second processing.

6. The method for estimating the posture of a robot-held acupuncture needle according to claim 1, characterized in that: The specific process of performing the second processing on the tactile image to generate the tactile point cloud includes: First, a lookup table is established by using the photometric stereo method, or the pixel intensity of the tactile image is converted into or pixel intensity and position As input, we get the gradient angle of the pixel point ( ), construct the corresponding gradient map ; Then, the corresponding depth map is solved from the gradient map through the 2D fast Poisson solver : ； in, To solve the error, the smaller the error, the better the depth map solved. The closer to the true depth; Finally, the depth map The depth value of each pixel in the depth map is compared with the preset significant threshold, and the pixels with depth lower than the significant threshold in the depth map are filtered out to obtain the final significant tactile point cloud.

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

8. The method for estimating the posture of a robot-held acupuncture needle according to claim 1, characterized in that: Based on tactile point cloud and And use ICP point cloud registration method to calculate the fine transformation matrix , specifically including: (1) Use KD-tree to perform nearest neighbor search from the transformed current tactile point cloud Tactile point cloud with target Select pairs of point cloud sets from , represented as and , and calculate their respective centroids: ； In the formula, Represents the current tactile point cloud Tactile point cloud with target The number of paired point clouds in and Represent the current tactile point cloud and target tactile point cloud In pairs point clouds; and The current tactile point cloud and target tactile point cloud The centroid of (2) Constructing paired point cloud sets and The covariance matrix of , and the covariance matrix Perform SVD decomposition: ； ； In the formula, , , Is The three matrices obtained by decomposition are is a diagonal matrix whose diagonal values ​​are singular values; , They are all orthogonal matrices; (3) Calculate the rotation matrix and translation vectors : ； ； Among them, the rotation matrix and translation vectors Constructing the fine transformation matrix ; (4) Calculate the transformed current tactile point cloud Tactile point cloud with target The distance error between : ； in, Indicates the sequence number of paired point clouds, and Indicates Point cloud; (5) Repeat steps (1) to (4) until the maximum number of iterations is reached or the current tactile point cloud after transformation is reached. Tactile point cloud with target The distance error between When it's small enough, stop iterating.

9. A posture estimation system for a mechanical handheld acupuncture needle based on visual-tactile perception, characterized in that: include: An image acquisition module is used to: use a manipulator gripper equipped with a visual tactile sensor to grasp acupuncture needles in two different postures, define the two different postures as the current posture and the target posture, and obtain the tactile image in the current posture and tactile image of the target posture ; The rough matching module is used to: and The first processing is performed to calculate the angle, length and position of the acupuncture needle in the two different posture tactile images, and based on the obtained angle, length and position, a rough matching matrix from the current posture to the target posture is calculated. ; Execution module, for: based on the rough matching matrix Control the robot gripper to rotate and translate in a 2D plane; The fine transformation module is used to: and Perform the second processing to generate the tactile point cloud under the current posture And tactile point cloud in target posture ; Then based on the rough matching matrix Tactile point cloud for the current posture Perform 2D posture alignment to obtain a roughly matched tactile point cloud under the current posture ; Based on the tactile point cloud and The ICP point cloud registration method is used to calculate the fine transformation matrix of the current tactile point cloud and the target tactile point cloud after 2D posture alignment. ; The execution module is also used for: based on the fine transformation matrix Control the robot gripper to rotate and translate in 3D space.

Images