Master hand operation arm kinematics accurate positioning visual verification method

Abstract

The present application relates to a master hand operating arm kinematic precise positioning visual verification method, which solves the technical problems of how to ensure the slave hand end to accurately and efficiently complete the expected planning path, and how to verify whether the action is accurate according to the expected planning path for the existing minimally invasive surgical robot based on master-slave mapping, which utilizes the DH parameterization method to obtain the analysis of the forward kinematic model of the master hand operating arm, and utilizes the simulink / ROS visualization technology to compare and verify the 3D dynamic simulation effect and the precise positioning accuracy of the mechanism end under the two methods of the master hand operating arm.

Description

Technical Field

[0001] This invention relates to the field of laparoscopic minimally invasive surgical robot technology, and more specifically, to a method for visual verification of precise kinematic positioning of the master arm. Background Technology

[0002] With continuous innovation in robotics technology, the technology of medical robots assisting medical staff in performing surgeries has developed rapidly. This is because medical robots can not only help medical staff perform a series of medical diagnoses and auxiliary treatments, but also effectively alleviate the problem of strained medical resources. Typically, a medical robot consists of a driven tool for performing the operation and a master manipulator for controlling the movement of the driven tool. In practical scenarios, the driven tool is configured to enter the operating area, and medical staff remotely operate the master manipulator to control the movement of the driven tool within the operating area, thus performing the medical procedure. Generally, medical robots achieve motion control of the driven tool by the master manipulator through a motion conversion between the master manipulator and the driven tool—a master-slave mapping.

[0003] Laparoscopic minimally invasive surgery refers to surgeries performed using modern medical instruments and equipment such as laparoscopes and thoracoscopes. Compared with traditional open surgery, minimally invasive surgery has advantages such as smaller incisions, less pain, less intraoperative bleeding, faster postoperative recovery, and lower risk of infection. Therefore, it has been widely recognized and applied in the field of surgery.

[0004] Referring to Chinese invention patents CN109091237B (Minimally Invasive Surgical Instrument Assistance System) and CN109091238B (Split-Type Minimally Invasive Surgical Instrument Assistance System), and utility model patent CN210872029U (Doctor's Operating Table), a minimally invasive surgical robot is designed as a device system operated by a doctor in the operating room under endoscopic guidance, controlling surgical instruments to perform surgery. The doctor sits at the operating table, views a 3D image display, and operates the robotic arm. The doctor can precisely control the surgical instruments to perform various surgical actions using the robotic arm, just like in a regular surgery.

[0005] Referring to invention patent applications with publication numbers CN105286999A and CN105286989A, surgical instruments can perform different functions, including clamping, resection, cutting, suturing, and anastomosis. Surgical instruments have different configurations, including an actuating end effector, wrist joint, instrument rod, and instrument case.

[0006] In existing minimally invasive surgical robot systems, doctors output motion commands via a handheld multi-degree-of-freedom mechanical motion input device. A master-slave motion mapping algorithm converts these commands into joint motion information for the robotic arm, allowing the slave robotic arm to follow the master robotic arm's movements. Reference patent CN112716608B discloses a DH master-slave mapping method.

[0007] When the master arm sends control commands to key joints, within the joint's range of motion, the kinematic relationship of the master arm can be mapped to the slave arm through a certain mapping ratio. This allows the surgeon to accurately and efficiently complete the surgical manipulation along the planned path within a set proportional framework. Therefore, ensuring the slave arm accurately and efficiently completes the planned path, and verifying whether the movements are accurately performed according to the planned path, are technical problems that urgently need to be solved by those skilled in the art. Summary of the Invention

[0008] This invention aims to solve the technical problems of how to ensure that the slave end of the minimally invasive surgical robot accurately and efficiently completes the expected planned path, and how to verify whether the action is accurately performed according to the expected planned path, based on the master-slave mapping. It provides a visualization verification method for precise kinematic positioning of the master arm.

[0009] In response to the need for precision medical surgical manipulation, this invention utilizes the DH parameterization method to obtain the analytical model of the forward kinematics of the master arm. Using Simulink / ROS visualization technology, the 3D dynamic simulation effect and the accuracy of precise end-effector positioning of the master arm under two different methods are compared and verified.

[0010] This invention provides a method for visually verifying the precise kinematic positioning of a master arm, comprising the following steps:

[0011] Step S1. Forward kinematics of the master arm based on the modified DH method;

[0012] Using the homogeneous transformation method to describe the pose of a rigid body, the kinematic relationships between links can be transformed into mathematical calculations. In the Cartesian coordinate system {A}, the position vector of any point P in space is described by a 3×1 vector. A P is:

[0013]

[0014] In the formula

[0015] P x —The x-component of point P along the x-axis;

[0016] P y —The component of point P along the y-axis;

[0017] P z —The z-component of point P;

[0018] A P—A represents the coordinate system {A};

[0019] To represent the orientation of any rigid body B in three-dimensional space, we use... This indicates the position of the origin of coordinate system {B} within coordinate system {A}. Indicate the orientation of coordinate system {B} within coordinate system {A}, and the orientation of rigid body B: The general form of a homogeneous matrix is:

[0020]

[0021] Step S2. Establish the kinematics of the master arm using the TF-Euler method;

[0022] The 3D model of the master arm is imported into the ROS system, and a kinematic system based on the TF tree is constructed to generate the mechanism model. From the modeling principle of the (TF) Euler transformation method, the purpose of the TF library is to realize the coordinate transformation of any point in the system between all coordinate systems. Therefore, taking the master arm base as an example... Construct a base coordinate system C with the bottom center as the origin. 0-0 ;

[0023] Step S3. Visual verification of the precise kinematic positioning solution of the master arm

[0024] Step 1. Comparison of single-point verification simulation results between the DH method and the TF method

[0025] 1) Importing TF data into the ROS system

[0026] On the ROS platform of the master manipulator, key kinematic data parameters of the TF coordinate system were set using a visual control interface to import TF data from the ROS system. Based on the key kinematic parameters of the TF coordinate system shown in Table 2, the parameters of the visual interface were adjusted, and the end effector position of the master manipulator was determined.

[0027]

[0028] 2) Alignment of the zero coordinate system

[0029] Comparing Table 1-2, the zero point of the joint rotation angle coordinate system around the z-axis, which differs from that of the (TF) Euler transformation, is...

[0030] TF0 = [0 0 0 0 0 0] T (17)

[0031] Coordinates under the DH transformation require an initial transformation.

[0032]

[0033] Due to the inconsistency of the initial states, the end position of the mechanism solved under the DH transformation satisfies the transformation rule: the x-axis coordinate value under the (TF) transformation is aligned with the y-axis coordinate value under the (DH) transformation, and the y-axis coordinate value (TF) under the (TF) transformation is aligned with the negative value (DH) of the x-axis coordinate value.

[0034]

[0035] 3) Solving the end position of the mechanism based on the zero-position alignment DH method

[0036] Combining the transformation matrix (19), the six joint rotation angle coordinates aligned under the DH transformation after the initial transformation (18) are zeroed out (the remaining parameters D) Hset Substituting {a1,d1,d3,d4,d5,d6} (relatively fixed) into the DH solution method represented by S1, the position of the end effector of the main arm is obtained:

[0037]

[0038] Step 2. Visual verification of precise kinematic positioning of the master arm based on two methods.

[0039] Using the ROS platform's visual control interface, key kinematic data parameters of the TF coordinate system are set, TF data import from the ROS system is realized, data parameters on the ROS system interface are visually controlled, key kinematic data of the mechanism are recorded under each parameter interface state, and the rotation angles of the six joints of the main hand manipulator and the end pose of the mechanism are obtained.

[0040] Under the description of kinematic parameters, the rotation angles and end positions of the six joints of the main hand manipulator were obtained respectively;

[0041] A Simulink-Simscape visualization interface for DH parameter control was designed.

[0042] Preferably, in step S1:

[0043] Step 1. Establish the DH coordinate system of the main manipulator.

[0044] The detailed steps for constructing the DH parametric coordinate system of the main manipulator's physical model are as follows:

[0045] (1) Construct a base coordinate system C with the bottom of the base as the origin. 0-0 ;

[0046] (2) Using the vertically upward z-axis as the coordinate axis, rotate counterclockwise by an angle θ1 around the z-axis and translate by a distance d1 along the z-axis to determine the first coordinate system C. 0-1 ;

[0047] (3) From z1 to z2, offset by a distance a1 along the x1 axis, rotate counterclockwise around the z-axis by an angle θ2, and then rotate clockwise. Determine the second coordinate system C 1-2 ;

[0048] (4) Rotate counterclockwise around the x-axis by an angle Rotate counterclockwise around the z-axis by an angle θ3, then rotate counterclockwise by another angle. Translate the coordinate system C by a distance d3 (the distance at which z2 and z4 intersect on the z3 axis) along the negative z-axis to determine the third coordinate system C. 2-3 ;

[0049] (5) Rotate counterclockwise around the x-axis by an angle α t Then rotate counterclockwise around the z-axis by an angle θ4, then rotate clockwise around the z-axis by an angle... Translate the coordinate system C along the positive z-axis by a distance d4 (the distance at which z3 and z5 intersect on the z4 axis), and determine the fourth coordinate system C. 3-4 ;

[0050] (6) Rotate clockwise around the x-axis by an angle Rotate counterclockwise around the z-axis by an angle θ5, then translate along the positive z-axis by a distance d5 (the distance at which z4 and z6 intersect on the z5 axis) to determine the fifth coordinate system C. 4-5 ;

[0051] (7) Rotate counterclockwise around the x-axis by an angle Rotate counterclockwise around the z-axis by an angle θ6, then translate by a distance d6 (the distance between the end point and the intersection of the z5 axis and the z6 axis) along the negative z-axis to determine the sixth coordinate system C. 5-6 .

[0052] Here, the concept of DH parameter transformation is used to establish a positive kinematic mapping relationship between the main arm and the end effector from the base to the end effector.

[0053] Step 2. Construct the DH parameter table for the master arm.

[0054] For any link i, the following definition is made:

[0055] a i Indicates the length of the link used: from z i-1 Joint axis to z i Vertical distance along the joint axis (along x) i The shaft offset distance is equivalent to the length of link i-1;

[0056] α i Indicates the linkage rotation angle: the angle between the axes of joint i-1 and joint i (around x). i By z i-1 Go to z i (rotation angle of the axis);

[0057] d i The offset distance of the link: the offset of link i relative to link i-1, that is, the distance between the two common perpendiculars — z i-1 With z i+1 Interchange with z i Distance from the axis (from x) i-1 To x i Along z i-1 (distance between axes);

[0058] θ i The joint angle of the link is the rotation angle of link i relative to link i-1 about the i-axis.

[0059] Step 3. Homogeneous transformation matrix of adjacent joints of the master arm

[0060] In a base coordinate system with the master arm base as the origin, the transformation rule of the modified DH method is an XZ-type transformation of the DH parameters (MDH transformation order is a→α→θ→d). Based on this rule, a forward kinematic mapping relationship based on the modified DH method can be established. Therefore, the homogeneous transformation matrix of adjacent joints in coordinate system {i} relative to coordinate system {i-1} is... The general formula is

[0061]

[0062] Among them, T z,d Relative to parameter d i Transformation, T z,θ Represents relative to parameter θ i Transformation, T x,a Relative to parameter a i Transformation, T x,α Represents relative to parameter α i The transformation, cθ i =cosθ i sθ i =sinθ i , i = 1, ..., 6.

[0063] Step 4. Mapping relationship of the master arm from the base to the end effector.

[0064] Here, the DH parameter configuration in Table 1 can be substituted into expression (3) to obtain the transfer matrix of each adjacent joint.

[0065]

[0066]

[0067]

[0068]

[0069]

[0070]

[0071] Considering the continuity of motion between adjacent joints, and combining expressions (4)-(9), the mathematical expression of the homogeneous transformation matrix from the base to the end of the mechanism is as follows:

[0072]

[0073] Among them, c i =cosθ i s i =sinθ i i = 1, ..., 6

[0074]

[0075] c ij =cos(θ) i +θ j ), s ij =sin(θ) i +θ j ), i≠j, i,j=1,2,

[0076]

[0077]

[0078]

[0079]

[0080]

[0081]

[0082]

[0083]

[0084]

[0085]

[0086]

[0087]

[0088] Represents the normal vector.

[0089] Represents the orientation vector, the direction of the line connecting the end-effector normal direction and the projection of the adjacent end joint onto the end-effector plane;

[0090] Representing the proximity vector, it determines the perpendicular direction of the line connecting the projections of adjacent joints onto the end-effector plane according to the right-hand rule;

[0091] Represents the position vector, relative to the origin of the base coordinate system, and the center position of the end effector of the master manipulator. Step 5. Master manipulator end effector position and attitude vector

[0092] The description and expression of the rigid body pose are described using the homogeneous transformation method (10), and the end position of the main hand manipulator is...

[0093]

[0094] in, {p x ,p y ,p z} is defined in (10).

[0095] Relative to expression (10), the normal direction of the end effector of the master arm for

[0096]

[0097]

[0098]

[0099] Here, det(@) represents the value of the matrix determinant. express The matrix consisting of the elements in the 2nd and 3rd rows / columns, express The matrix consisting of the elements in the 1st and 3rd rows / columns, express The matrix consists of the elements in the first and second rows / columns. Combining formulas (10), (12)-(14), the normal direction of the end effector of the main hand manipulator is calculated.

[0100]

[0101] in,

[0102]

[0103]

[0104] The beneficial effects of this invention are: enabling doctors to accurately and efficiently complete the surgical manipulation of the desired planned path under the set mapping ratio scenario, solving the problem of the difficulty in aligning and positioning medical instruments with the patient's worktable in existing procedures.

[0105] This invention not only decouples the solution of the overall kinematics of the master manipulator through modular design, but also uses the DH method to determine the spatial position of the end effector, facilitating the establishment of a master-slave mapping relationship and solving the problem of difficult alignment and positioning between existing medical instruments and operating tables. This invention also provides important theoretical support for subsequent issues such as motion control strategies for multi-arm robots. Therefore, it has wide applications in industrial robots, medical robots, and space robots.

[0106] Further features and aspects of the present invention will be clearly described in the following detailed description with reference to the accompanying drawings. Attached Figure Description

[0107] Figure 1 This is the physical model of the main arm of the present invention in the DH coordinate system;

[0108] Figure 2 This is a layout diagram of the visual control interface of the main arm ROS platform of this invention;

[0109] Figure 3 This invention relates to the rotational joint angle of the main hand manipulator arm under TF transformation.

[0110] Figure 4 This is the end-effector coordinate position of the main arm of the present invention under TF transformation;

[0111] Figure 5 This is a layout diagram of the Simulink-Simscape visualization interface under the DH transformation of the main hand manipulator of this invention;

[0112] Figure 6 This invention aligns the rotational joint angle coordinate system under the DH transformation of the main hand operating arm.

[0113] Figure 7 This invention relates to the control flow of the main hand manipulator visualization verification method.

[0114] Figure 8 This is the position error curve of the alignment between the TF transformation and DH transformation coordinate systems of the main hand manipulator of this invention;

[0115] Figure 9 This is a table of kinematic parameters for the DH and TF coordinate systems with zero alignment. Detailed Implementation

[0116] The actual test was conducted using two types of data: MATLAB simulation data and semi-physical simulation image data. The specific implementation method is as follows:

[0117] Step S1. Forward kinematics of the master arm based on the modified DH method

[0118] The core idea of ​​the DH method for solving the forward kinematics of the master manipulator is as follows: First, the DH method is used to model the forward kinematics of the master manipulator. Second, the model is analyzed to calculate the spatial transformation and positional transfer relationships between each joint and the end effector. Finally, the end effector output posture information is obtained. The specific process is as follows:

[0119] Here, the homogeneous transformation method is used to describe the pose of the rigid body, which transforms the kinematic relationships between links into mathematical calculations. In the Cartesian coordinate system {A}, any point P in space is described by a 3×1 vector as its position vector. A P is:

[0120]

[0121] In the formula

[0122] P x —The x-component of point P along the x-axis;

[0123] P y —The component of point P along the y-axis;

[0124] P z —The z-component of point P;

[0125] A P——A represents the coordinate system {A}.

[0126] To represent the orientation of any rigid body B in three-dimensional space, we use... This indicates the position of the origin of coordinate system {B} within coordinate system {A}. Indicate the orientation of coordinate system {B} within coordinate system {A}, and the orientation of rigid body B: The general form of a homogeneous matrix is:

[0127]

[0128] Step 1. Establish the DH coordinate system of the main manipulator.

[0129] See Figure 1 The detailed steps for constructing the DH parametric coordinate system of the main manipulator's physical model are as follows:

[0130] (1) Construct a base coordinate system C with the bottom of the base as the origin. 0-0 ;

[0131] (2) Using the vertically upward z-axis as the coordinate axis, rotate counterclockwise by an angle θ1 around the z-axis and translate by a distance d1 along the z-axis to determine the first coordinate system C. 0-1 ;

[0132] (3) From z1 to z2, offset by a distance a1 along the x1 axis, rotate counterclockwise around the z-axis by an angle θ2, and then rotate clockwise. Determine the second coordinate system C 1-2 ;

[0133] (4) Rotate counterclockwise around the x-axis by an angle Rotate counterclockwise around the z-axis by an angle θ3, then rotate counterclockwise by another angle. Translate the coordinate system C by a distance d3 (the distance at which z2 and z4 intersect on the z3 axis) along the negative z-axis to determine the third coordinate system C. 2-3 ;

[0134] (5) Rotate counterclockwise around the x-axis by an angle α t Then rotate counterclockwise around the z-axis by an angle θ4, then rotate clockwise around the z-axis by an angle... Translate the coordinate system C along the positive z-axis by a distance d4 (the distance at which z3 and z5 intersect on the z4 axis), and determine the fourth coordinate system C. 3-4 ;

[0135] (6) Rotate clockwise around the x-axis by an angle Rotate counterclockwise around the z-axis by an angle θ5, then translate along the positive z-axis by a distance d5 (the distance at which z4 and z6 intersect on the z5 axis) to determine the fifth coordinate system C. 4-5 ;

[0136] (7) Rotate counterclockwise around the x-axis by an angle Rotate counterclockwise around the z-axis by an angle θ6, then translate by a distance d6 (the distance between the end point and the intersection of the z5 axis and the z6 axis) along the negative z-axis to determine the sixth coordinate system C. 5-6 .

[0137] Here, the concept of DH parameter transformation is used to establish a positive kinematic mapping relationship between the main arm and the end effector from the base to the end effector.

[0138] Step 2. Construct the DH parameter table for the master arm.

[0139] Table 1. DH Parameter Configuration Table for Master Hand Manipulator

[0140]

[0141] Note: Based on actual measurements, the DH parameters in Table 1 can be determined. Here, for any link i, the following definition is made:

[0142] a i Indicates the length of the link used: from z i-1Joint axis to z i Vertical distance along the joint axis (along x) i The shaft offset distance is equivalent to the length of link i-1;

[0143] α i Indicates the linkage rotation angle: the angle between the axes of joint i-1 and joint i (around x). i By z i-1 Go to z i (rotation angle of the axis);

[0144] d i The offset distance of the link: the offset of link i relative to link i-1, that is, the distance between the two common perpendiculars — z i-1 With z i+1 Interchange with z i Distance from the axis (from x) i-1 To x i Along z i-1 (distance between axes);

[0145] θ i The joint angle of a link is: the rotation angle of link i relative to link i-1 about the i-axis (around the z-axis). i By x i-1 Go to x i (The angle of rotation of the axis).

[0146] Step 3. Homogeneous transformation matrix of adjacent joints of the master arm

[0147] Here, in the base coordinate system with the master arm base as the origin, the transformation rule of the modified DH method is the XZ-type transformation of the DH parameters (MDH transformation order is a→α→θ→d). Based on this rule, a positive kinematic mapping relationship based on the modified DH method can be established. Therefore, the homogeneous transformation matrix of adjacent joints in coordinate system {i} relative to coordinate system {i-1} is... The general formula is

[0148]

[0149] Among them, T z,d Relative to parameter d i Transformation, T z,θ Represents relative to parameter θ i Transformation, T x,a Relative to parameter a i Transformation, T x,α Represents relative to parameter α i The transformation, cθ i =cosθ i sθ i =sinθ i , i = 1, ..., 6.

[0150] Step 4. Mapping relationship of the master arm from the base to the end effector.

[0151] Here, the DH parameter configuration in Table 1 can be substituted into expression (3) to obtain the transfer matrix of each adjacent joint.

[0152]

[0153]

[0154]

[0155]

[0156]

[0157]

[0158] Considering the continuity of motion between adjacent joints, and combining expressions (4)-(9), the mathematical expression of the homogeneous transformation matrix from the base to the end of the mechanism is as follows:

[0159]

[0160] Among them, c i =cosθ i s i =sinθ i i = 1, ..., 6

[0161]

[0162] c ij =cos(θ) i +θ j ), s ij =sin(θ) i +θ j ), i≠j, i,j=1,2,

[0163]

[0164]

[0165]

[0166]

[0167]

[0168]

[0169]

[0170]

[0171]

[0172]

[0173]

[0174]

[0175] Represents the normal vector.

[0176] Represents the orientation vector, the direction of the line connecting the end-effector normal direction and the projection of the adjacent end joint onto the end-effector plane;

[0177] Representing the proximity vector, it determines the perpendicular direction of the line connecting the projections of adjacent joints onto the end-effector plane according to the right-hand rule;

[0178] Represents the position vector, relative to the origin of the base coordinate system, and the center position of the end effector of the master arm.

[0179] Step 5. Position and attitude vector of the master arm end effector

[0180] The description and expression of the rigid body pose are described using the homogeneous transformation method (10), and the end position of the main hand manipulator is...

[0181]

[0182] in, {p x ,p y ,p z} is defined in (10).

[0183] Relative to expression (10), the normal direction of the end effector of the master arm for

[0184]

[0185]

[0186]

[0187] Here, det(·) represents the value of the matrix determinant. express The matrix consisting of the elements in the 2nd and 3rd rows / columns, express The matrix consisting of the elements in the 1st and 3rd rows / columns, express The matrix consists of the elements in the first and second rows / columns. Combining formulas (10), (12)-(14), the normal direction of the end effector of the main hand manipulator is calculated.

[0188]

[0189] in,

[0190]

[0191]

[0192]

[0193] Note: The accuracy of the kinematics solution for the master arm established using the DH method when importing the mechanical model needs further verification. This is because there is always a certain error between the theoretical calculations and the actual measurement data. Under the current experimental conditions, a relatively accurate ROS simulation platform has been built, and the actual kinematic data of the master arm is simulated using the (TF) Euler transform method.

[0194] Step S2. Establish the kinematics of the master arm using the TF-Euler method.

[0195] Here, the 3D model of the main manipulator is imported into the ROS system to construct a kinematic system based on the TF tree to generate the mechanism model. From the modeling principle of the (TF) Euler transformation method, the purpose of the TF library is to realize the coordinate transformation of any point in the system between all coordinate systems. Therefore, taking the main manipulator base... Construct a base coordinate system C with the bottom center as the origin. 0-0 The TF coordinate system is constructed as shown in Table 2.

[0196] Table 2. Key kinematic parameters of the TF coordinate system

[0197]

[0198] Step S3. Visual verification of the precise kinematic positioning solution of the master arm

[0199] Step 1. Comparison of single-point verification simulation results between the DH method and the TF method

[0200] 1) Importing TF data into the ROS system

[0201] On the ROS platform of the master manipulator, key kinematic data parameters of the TF coordinate system were set using a visual control interface to import TF data into the ROS system. Based on the key kinematic parameters of the TF coordinate system shown in Table 2, the parameters of the visual interface were adjusted, and the end effector position of the master manipulator was determined.

[0202]

[0203] 2) Alignment of the zero coordinate system

[0204] Comparing Table 1-2, the zero point of the joint rotation angle coordinate system around the z-axis, which differs from that of the (TF) Euler transformation, is...

[0205] TF0 = [0 0 0 0 0 0] T (17)

[0206] Coordinates under the DH transformation require an initial transformation.

[0207]

[0208] Due to the inconsistency of the initial states, the end position of the mechanism solved under the DH transformation satisfies the transformation rule: the x-axis coordinate value under the (TF) transformation is aligned with the y-axis coordinate value under the (DH) transformation, and the y-axis coordinate value (TF) under the (TF) transformation is aligned with the negative value (DH) of the x-axis coordinate value.

[0209]

[0210] 3) Solving the end position of the mechanism based on the zero-position alignment DH method

[0211] Combining the transformation matrix (19), the six joint rotation angle coordinates aligned under the DH transformation after the initial transformation (18) are zeroed out (the remaining parameters D) Hset Substituting {a1,d1,d3,d4,d5,d6} (relatively fixed) into the DH solution method represented by S1, the position of the end effector of the main arm is obtained:

[0212]

[0213] Summary: Comparing the simulated end-effector position data under the ROS simulation platform, the positional errors obtained by our proposed DH method are as follows: average error of 1mm on the x-axis, 0.1mm on the y-axis, and 0.01mm on the z-axis. This numerical simulation result shows that the positional error of the master manipulator kinematics single-point verification is sufficiently small, but this does not fully demonstrate the feasibility of our adopted DH method. Below, we will use the following visualization method for precise kinematic positioning of the master manipulator to further verify the positional accuracy. Step 2. Visual Verification of Precise Kinematic Positioning of the Master Manipulator Based on Two Methods

[0214] See Figure 2 Using the ROS platform's visual control interface, key kinematic data parameters in the TF coordinate system were set, enabling the import of TF data into the ROS system. To further verify the accuracy of the end effector position of the master manipulator, data parameters were visually controlled on the ROS system interface, and key kinematic data of the mechanism were recorded under each parameter interface state, obtaining the rotation angles of the six joints of the master manipulator and the end effector pose. Here, 18 sets of TF data were selected. Through analysis and calculation, it was found that two sets of data were singular unreachable points under the DH method, therefore, 16 sets of data were valid. The kinematic parameter coordinate system of the DH coordinate system and the TF coordinate system with zero alignment was constructed as follows. Figure 9 As shown in the table.

[0215] See Figure 3 4. Under the kinematic parameter description in Table 3, the rotation angles and end positions of the six joints of the main hand manipulator are obtained based on 16 sets of valid data using TF transformation.

[0216] See Figure 5 A Simulink-Simscape visualization interface for DH parameter control was designed.

[0217] See Figure 6 Referring to the method in step 1 above, the end position of the mechanism under the zero position of the coordinate system with different joint rotation angles under the DH method is deduced in turn, so as to realize the alignment of the TF transformation coordinate system with the DH coordinate system.

[0218] See Figure 7 Based on the above conclusions, the control process of the visualization verification method is summarized.

[0219] See Figure 8 ,application Figure 7 The visualization verification method controls the flow, and the final calculated results show that the coordinate values ​​projected onto the x-axis and y-axis are controlled within a narrow range of [0.02, 0.04] mm. Except for the z-axis position error, which fluctuates significantly in sets 14-16, the remaining z-axis coordinate values ​​are also controlled within this range. Therefore... Figure 8 The described end-effector position error is asymptotically convergent. Theoretical calculations show that the average error along the x-axis is... The average error of the y-axis is The average error along the z-axis is The accuracy index of error location is calculated based on the calculation formula of the medical industry indicator evaluation standard.

[0220]

[0221] in,

[0222] The calculation result of this indicator (21) meets the control accuracy requirement of <2mm, which means it meets the design requirements for position accuracy control of the surgical equipment. This result shows that the visualization verification method for precise kinematic positioning of the master arm proposed in this invention ensures that the final obtained position accuracy meets the surgical design requirements and conforms to the position accuracy verification index requirements for motion control of the surgical equipment.

[0223] It should be noted that the specific method of master-slave mapping based on the FH method can also adopt the method in the invention patent with authorization announcement number CN112716608B.

Claims

1. A master hand manipulator kinematic precise positioning visual verification method, characterized by, Comprising the following steps: Step S1. Master manipulator forward kinematics based on modified DH method; The homogeneous transformation method is used to describe the pose of rigid body, and the motion relationship between the connecting rods is converted into mathematical calculation. In the Cartesian coordinate system , the position vector of any point in space is described by vector : （1） In the formula, — component of point P in the x-axis direction; — component of point P in the y-axis direction; - the component of the point P in the z-axis direction; - A denotes a coordinate system ; To represent the pose of an arbitrary rigid body B in three-dimensional space, we use to represent the position of the origin of the coordinate system in the coordinate system , and to represent the orientation of the coordinate axes of the coordinate system in the coordinate system , the pose of the rigid body B: The general form of the homogeneous matrix is （2） Step S2. TF Euler method establishes master manipulator kinematics; The master hand operating arm 3D model is imported into the ROS system, and a TF tree-based generating mechanism model kinematics system is constructed. From the modeling principle of the TF Euler transformation method, the purpose of the TF library is to realize the coordinate transformation of any point in the system between all coordinate systems. Therefore, the base of the master hand operating arm is taken as the origin to construct the base coordinate system. The base of the master hand operating arm is taken as the origin to construct the base coordinate system. ; Step S3. Visual verification of master manipulator kinematics precise positioning solution Step 1. Single-point verification simulation result comparison between DH method and TF method 1) TF data import of ROS system Table 2. Key kinematic parameters of TF coordinate system ， On the built master manipulator ROS platform, the key kinematic data parameters of the TF coordinate system are set by using the visual control interface, the TF data import of the ROS system is realized, the key kinematic parameters of the TF coordinate system are shown in Table 2, the visual interface parameters are controlled, and the master manipulator end position is （16） 2) Zero position alignment Comparing Tables 1-2, the zero position of the joint rotation angle coordinate system around the z axis in the TF Euler transformation is （17） The coordinates under the DH transformation need to undergo initial transformation （18） Due to the inconsistency of the initial state, the mechanism end position solved under the DH transformation satisfies the transformation rule: the x-axis coordinate value under the TF transformation is aligned with the y-axis coordinate value under the DH transformation, and the y-axis coordinate value under the TF transformation is aligned with the negative value of the x-axis coordinate value under the DH transformation; （19） 3) Mechanism end position solution based on zero position alignment DH method Combining the transformation matrix (19), the DH transformation after the initial transformation (18) is aligned with the 6 joint rotation angle coordinate system zero position, and the remaining parameters Substitute into the DH solving method represented by step S1, the master hand operating arm end position: （20）； Step 2. Visual verification of master manipulator kinematics precise positioning based on two methods Under the description of kinematic parameters, the master manipulator 6 joint rotation angles and end position are obtained respectively; The DH parameter control Simulink-Simscape visual interface is planned out; In the step S1: Step 1. Build master manipulator DH coordinate system The detailed steps of the DH parameter coordinate system construction criteria under the master manipulator DH coordinate system physical model are as follows: (1) Take the base bottom end as the origin, construct the base coordinate system ; (2) the z axis in the vertical upward direction, rotating the angle counterclockwise around the z axis , translating the distance along the z axis , determining the first coordinate system ; (3) from to along an axis offset distance , rotate the angle clockwise around the z axis , determine the second coordinate system ; (4) Rotate counterclockwise around the x-axis by an angle Rotate counterclockwise around the z-axis by an angle Then rotate the angle counterclockwise. Translation distance along the negative z-axis , and Hand over The distance between the axes determines the third coordinate system. ; (5) rotate around x axis counterclockwise by angle , rotate around z axis counterclockwise by angle , rotate around z axis clockwise by angle , translate along z axis positive direction by distance , and intersect at axis, determine the 4th coordinate system ; (6) rotate clockwise around x axis by angle , rotate counterclockwise around z axis by angle , translate along z axis by distance , with intersecting at axis, determine the 5th coordinate system ; (7) rotate about x-axis counterclockwise by angle , rotate about z-axis counterclockwise by angle , translate along z-axis negative direction by distance , define end point with axis intersect at axis, determine 6th coordinate system , Here, the idea of DH parameter transformation is used to build the forward kinematics mapping relationship of the master manipulator from the base to the mechanism end; Step 2. Construct master manipulator DH parameter table For any link i, define as follows: denotes the length of the link: the distance from the joint axis to the perpendicular distance from the joint axis, along the axis offset distance, equivalent to the length of the link ; represents the angle of rotation of the connecting rod: the angle between the joint and the joint axis, about by turn to angle of rotation of the shaft; Indicates the offset distance of the link: link i relative to link The offset, that is, the distance between the two common perpendiculars— and Hand over The distance along the axis, from arrive along Distance between axes; denotes the joint angle of link i with respect to link the rotation angle around the i-axis; Step 3. Master manipulator adjacent joint homogeneous transformation matrix In the base coordinate system with the base of the master hand operating arm as the origin, the modified DH method transformation rule is XZ type transformation of DH parameters, and the MDH transformation sequence is According to the rule, the forward kinematics mapping relationship based on the modified DH method can be established, and thus the homogeneous transformation matrix of the adjacent joint of the coordinate system with respect to the coordinate system is ​ ， （3） wherein represents a transformation with respect to parameters , represents a transformation with respect to parameters , represents a transformation with respect to parameters , represents a transformation with respect to parameters , , , ; Step 4. Mapping relationship of master manipulator from base to mechanism end Table 1. Master manipulator DH parameter configuration table ， Here, referring to the DH parameter configuration in Table 1, each adjacent joint transfer matrix is obtained by substituting the expression (3) （4） （5） （6） （7） （8） （9） Considering the continuity of adjacent joint motion, combined with expressions (4)-(9), the homogeneous transformation matrix from the base to the mechanism end is mathematically expressed as （10） wherein , , , , , , , , , ； ； ； ； ； ； ； ； ； ； ； ； representative normal vector, ; representative orientation vector, the direction of the line connecting the projection of the end-effector orientation and the joint adjacent to the end-effector on the end-effector plane; representing the approaching vector, determined according to the right-hand rule, the perpendicular direction of the projection line direction of the end adjacent joint on the end plane; representative position vector, relative to the origin of the base coordinate system, center of the end of the master manipulator arm, for rotating counterclockwise around the x-axis by an angle; Step 5. Master manipulator end position and attitude vector Using the description and expression (10) of rigid body pose described by homogeneous transformation method, the master manipulator end position is （11） wherein , defined in (10); Master hand operating arm tip direction is （12） （13） （14） Here, denotes the value of the matrix determinant, denotes a matrix consisting of the 2nd, 3rd row / column elements in represents a matrix composed of the 1st, 3rd row / column elements in the represents a matrix composed of the 1st, 2nd row / column elements in the in combination with equations (10), (12)-(14) （15） wherein, ， ， 。

Images