阅读说明：本技术 用于微创手术机器人的主从跟踪控制方法 (Master-slave tracking control method for minimally invasive surgery robot ) 是由 姜英俊 王炳强 王淑林 孙之建 于 2021-01-20 设计创作，主要内容包括：本发明涉及一种用于微创手术机器人的主从跟踪控制方法,其解决现有微创手术机器人系统主从跟踪控制方法实时性差、准确性差、精度低的技术问题,其将异构映射模式与同构映射模式相结合的简化运动学建模方法,主从操作手定位部分采用异构映射模式,主从操作手姿态部分采用同构映射模式。本发明广泛用于微创手术机器人。(The invention relates to a master-slave tracking control method for a minimally invasive surgery robot, which solves the technical problems of poor real-time performance, poor accuracy and low precision of the master-slave tracking control method of the existing minimally invasive surgery robot system. The invention is widely applied to minimally invasive surgery robots.)

1. A master-slave tracking control method for a minimally invasive surgery robot is characterized by comprising the following steps:

establishing a D-H parameter table of the main operating arm by using a D-H method, wherein the D-H parameter table comprises the connecting rod lengths a of three joints of M1, M2 and M3_iOffset d of the connecting rod_iAngle of rotation of connecting rod alpha_MiAnd joint angle theta_MiAccording to the principle of space transformation, a homogeneous transformation matrix between two adjacent joints is obtained according to the following formula:

establishing transformation between each adjacent joint according to the above formula, and then solving a homogeneous transformation matrix of the terminal coordinate system of the main operating arm relative to the base coordinate system according to the following formula

Obtaining the position of the tail end of the main operating arm relative to the base coordinate system from the homogeneous transformation matrix;

will be provided withWritten in this form:

then, the following steps are carried out:

P_Mthe position coordinates of the wrist of the main operation arm are obtained;

establishing a corresponding dynamic coordinate system at the tail end position point of the main operating arm, and calculating the rotation angle values theta of the three joints M4, M5 and M6 of the wrist according to the following formula_MiIndicating the posture of the wrist 4;

θ_Mi＝A_Mi/μ_Mi

wherein A is_MiRepresents the amount of change, mu, of the joint encoder value_MiThe product of the joint encoder precision, the speed reduction ratio of the speed reducer and the transmission ratio of the mechanical structure;

the joints S1, S2, and L3 of the slave manipulator, the joints S4, S5, and S6 of the wrist at the end of the surgical instrument, and the slave manipulatorThe arm end adopts a D-H parameter method to establish a kinematic model, and a homogeneous transformation matrix of the tail end of the operation arm in the base coordinate system is obtained according to a homogeneous transformation matrix calculation formulaThe rotation angle values theta of the three joints of the wrist of the surgical instrument are calculated according to the following formula_Si：

θ_Si＝A_Si/μ_Si

Mapping the position base coordinate system of the master operating arm into the base coordinate system described by the slave operating arm, and obtaining a homogeneous transformation matrix of the slave operating arm after mapping as follows:

wherein the content of the first and second substances,is a transformation matrix from the base coordinate system of the master operation arm to the base coordinate system of the slave operation arm,scale factors for master-slave mapping;

written in this form:

from the position coordinates of the operating armAccording to a homogeneous transformation matrix of the joints S1, S2 and L3 of the manipulator arm, which is derived from the configuration of the manipulator arm according to a D-H parameter method:

wherein, the following relation is further established:

p_sx＝x_s

p_sy＝y_s

p_sz＝z_s

according to the three equations, the unknown parameters theta of the joint S1, the joint S2 and the joint L3 can be obtained_s1、θ_s2、l_s3；

The calculation formula of the expected angle values of the wrist three joints at the tail end of the operation arm instrument is as follows:

θ_s4＝θ_M4

θ_s5＝θ_M5

θ_s6＝θ_M6

wherein, theta_M4、θ_M5、θ_M6Respectively representing the actual rotation angle values of three joints of the main operation wrist part;

and then the motion controller is used for outputting a corresponding numerical value to the slave operation arm to realize master-slave following.

Technical Field

The invention relates to the technical field of minimally invasive surgery robots, in particular to a master-slave tracking control method for a minimally invasive surgery robot.

Background

The minimally invasive surgical robot is designed to be operated in an operating room by a doctor under an endoscope, and is controlled by the doctor to operate the surgical instrument to perform an operation. The doctor sits in front of the doctor operation panel, and through watching the 3D image display, operates the doctor arm, and the doctor can utilize the accurate control surgical instruments of patient arm to carry out various operation actions like ordinary operation.

Referring to the invention patent application publication Nos. CN105286999A, CN105286989A, surgical instruments may perform various functions including clamping, cutting, stapling, and the like. Surgical instruments come in different configurations, including an execution tip, wrist, instrument shaft, instrument box, and the like.

In the existing minimally invasive surgery robot system, a doctor outputs an action command by holding a mechanical motion input device with multiple degrees of freedom, the collected action command can be converted into joint motion information of a mechanical arm through a master-slave motion mapping algorithm, the mechanical arm follows the motion of a master mechanical arm, and the current algorithm is poor in real-time performance, accuracy and precision.

Disclosure of Invention

The invention provides a master-slave tracking control method for a minimally invasive surgery robot, which aims to solve the technical problems of poor instantaneity, poor accuracy and low precision of the master-slave tracking control method of the existing minimally invasive surgery robot system and improve the poor instantaneity, poor accuracy and precision.

The invention provides a master-slave tracking control method for a minimally invasive surgery robot, which comprises the following steps:

establishing a D-H parameter table of the main operating arm by using a D-H method, wherein the D-H parameter table comprises the connecting rod lengths a of three joints of M1, M2 and M3_iOffset d of the connecting rod_iAngle of rotation of connecting rod alpha_MiAnd joint angle theta_MiAccording to the principle of space transformation, a homogeneous transformation matrix between two adjacent joints is obtained according to the following formula:

establishing transformation between each adjacent joint according to the above formula, and then solving a homogeneous transformation matrix of the terminal coordinate system of the main operating arm relative to the base coordinate system according to the following formula

Obtaining the position of the tail end of the main operating arm relative to the base coordinate system from the homogeneous transformation matrix;

will be provided withWritten in this form:

then, the following steps are carried out:

P_Mthe position coordinates of the wrist of the main operation arm are obtained;

establishing a corresponding dynamic coordinate system at the tail end position point of the main operating arm, and calculating the rotation angle values theta of the three joints M4, M5 and M6 of the wrist according to the following formula_MiIndicating the posture of the wrist 4;

θ_Mi＝A_Mi/μ_Mi

wherein A is_MiRepresents the amount of change, mu, of the joint encoder value_MiThe product of the joint encoder precision, the speed reduction ratio of the speed reducer and the transmission ratio of the mechanical structure;

a joint S1, a joint S2 and a joint L3 of the slave operation arm, a joint S4, a joint S5 and a joint S6 of the wrist at the tail end of the surgical instrument adopt a D-H parameter method to establish a kinematic model aiming at the slave operation arm end,obtaining a homogeneous transformation matrix of the tail end of the operation arm in the base coordinate system according to the homogeneous transformation matrix calculation formulaThe rotation angle values theta of the three joints of the wrist of the surgical instrument are calculated according to the following formula_Si：

θ_Si＝A_Si/μ_Si

Mapping the position base coordinate system of the master operating arm into the base coordinate system described by the slave operating arm, and obtaining a homogeneous transformation matrix of the slave operating arm after mapping as follows:

wherein the content of the first and second substances,is a transformation matrix from the base coordinate system of the master operation arm to the base coordinate system of the slave operation arm,scale factors for master-slave mapping;

written in this form:

from the position coordinates of the operating armAccording to a homogeneous transformation matrix of the joints S1, S2 and L3 of the manipulator arm, which is derived from the configuration of the manipulator arm according to a D-H parameter method:

wherein, the following relation is further established:

p_sx＝x_s

p_sy＝y_s

p_sz＝z_s

according to the three equations, the unknown parameters theta of the joint S1, the joint S2 and the joint L3 can be obtained_s1、θ_s2、l_s3；

The calculation formula of the expected angle values of the wrist three joints at the tail end of the operation arm instrument is as follows:

θ_s4＝θ_M4

θ_s5＝θ_M5

θ_s6＝θ_M6

wherein, theta_M4、θ_M5、θ_M6Respectively representing the actual rotation angle values of three joints of the main operation wrist part;

and then the motion controller is used for outputting a corresponding numerical value to the slave operation arm to realize master-slave following.

The invention has the beneficial effects that: the invention discloses a simplified kinematic modeling method combining a heterogeneous mapping mode and a homogeneous mapping mode. The positioning part of the master-slave manipulator adopts a heterogeneous mapping mode, and the posture part of the master-slave manipulator adopts an isomorphic mapping mode. The complexity of the whole kinematics calculation process is simplified, and the real-time performance, accuracy and precision of the master-slave tracking motion control system are improved.

Further features and aspects of the present invention will become apparent from the following description of specific embodiments with reference to the accompanying drawings.

Drawings

FIG. 1 is a schematic structural diagram of a main operating arm of a minimally invasive surgical robot;

FIG. 2 is a schematic diagram of the structure of the wrist shown in FIG. 1;

FIG. 3 is a block diagram of the wrist shown in FIG. 2;

FIG. 4 is a configuration diagram of the main operating arm position adjustment;

FIG. 5 is a schematic view of the slave operating arm in a folded state;

FIG. 6 is a schematic view of the slave arm in a deployed state;

FIG. 7 is a schematic view of the slave arm in a deployed state;

FIG. 8 is a configuration view of the slave arm;

FIG. 9 is a configuration diagram of a surgical instrument;

FIG. 10 is a mechanical schematic of the surgical instrument;

FIG. 11 is a configuration diagram of the wrist.

The symbols in the drawings illustrate that:

1. the device comprises a base, 2, a first arm rod, 3, a second arm rod, 4, a wrist, 4-1, a fixed connecting rod, 4-2, a first L-shaped connecting rod, 4-3, a second L-shaped connecting rod, 4-4, a handle, 4-5, an opening and closing seat, 4-6, a first encoder, 4-7, a second encoder and 4-8, a third encoder, wherein the first arm rod is arranged on the base; 5. the connecting seat is rotated. 201. The base end base, 202, the connecting rod base, 203, the slave end connecting rod I, 204, the slave end connecting rod II, 205, the instrument installation device, 206, the instrument lifting base, 207, the surgical instrument, 208, the motor, 209 and the motor.

Detailed Description

The main operating arm applied to the invention is a doctor mechanical arm in the utility model patent with the name of doctor operating table and the publication number of CN 210872029U. The slave manipulator arm used is the mechanical arm of patent No. 201711314221.6, named the master manipulator arm for minimally invasive surgery.

As shown in fig. 1, the rear end of the first arm lever 2 is connected to the base 1 through a first joint, the first arm lever 2 can rotate on the horizontal plane, the rotation connecting base 5 is connected to the front end of the first arm lever 2 through a second joint, the rotation connecting base 5 can rotate on the horizontal plane, the second arm lever 3 is connected to the rotation connecting base 5 through a third joint, and the second arm lever 3 can rotate on the vertical plane.

In the operation process, a doctor holds the wrist 4 with a hand to operate, and the joints of the main operation arm rotate around a first joint axis 14 of the first joint, a second joint axis 15 of the second joint and a third joint axis 16 of the third joint respectively, wherein the third joint axis 16 is vertical to the gravity direction, and the first joint axis 14 and the second joint axis 15 are vertical to the ground.

As shown in figures 2 and 3, the wrist 4 comprises a fixed connecting rod 4-1, a first L-shaped connecting rod 4-2, a second L-shaped connecting rod 4-3, a handle 4-4, an opening and closing seat 4-5, a first encoder 4-6, a second encoder 4-7 and a third encoder 4-8. The wrist 4 is provided with a joint M4, a joint M5 and a joint M6.

Fig. 4 shows a main operation arm position adjustment configuration diagram, wherein the three joints M1, M2 and M3 are the first joint, the second joint and the third joint in the configuration diagram shown in fig. 1. As shown in fig. 3 and 11, three joints M4, M5 and M6 are attitude adjusting joints, and the degrees of freedom of these 6 joints respectively determine the position and attitude of the end of the main operating arm in a cartesian coordinate system. According to the configuration of the main operating arm, the kinematic analysis of the main operating arm can be carried out by adopting a D-H method, and a D-H parameter table is established, wherein the D-H parameter table comprises the connecting rod lengths a of three joints of M1, M2 and M3_iOffset d of the connecting rod_iAngle of rotation of connecting rod alpha_MiAnd joint angle theta_Mi. For example, in FIG. 1, a₁Denotes the distance, a, between the axis of the joint M1 and the axis of the joint M2₂Denotes the distance, a, between the axis of the joint M2 and the axis of the joint M3₃Indicating the distance between the axis of the joint M3 and the wrist 4. The link offset is the distance between two adjacent common perpendicular lines along the axial direction, and the link offset d_iAre all 0. Knowing the parameters of each joint, a homogeneous transformation matrix between two adjacent joints is obtained according to the principle of spatial transformation as follows.

The transformation between each adjacent joint is established according to the above formula, and then the controller can obtain a homogeneous transformation matrix of the coordinate system of the tail end of the main operation arm (namely the coordinate system of the wrist 4) relative to the base coordinate system according to the following formulaAnd i takes values of 1, 2 and 3, and the position of the tail end of the main operating arm relative to the base coordinate system can be obtained from the homogeneous transformation matrix.

The following can also be written:

then, the following steps are carried out:

P_Mi.e. the position coordinates of the wrist 4 of the main operating arm.

Establishing a corresponding dynamic coordinate system at the tail end position point of the main operating arm, and calculating the rotation angle values theta of the three joints M4, M5 and M6 of the wrist 4 according to the following formula_MiAnd represents the posture of the wrist 4.

θ_Mi＝A_Mi/μ_Mi

Wherein A is_MiRepresents the amount of change, mu, of the joint encoder value_MiThe product of the joint encoder precision, the speed reduction ratio of the speed reducer and the transmission ratio of the mechanical structure. Referring to FIG. 3, θ_M4For the rotation angle value of the joint M4, the corresponding encoder is the first encoder 4-6, theta_M5For the value of the rotation angle of the joint M5, the corresponding encoder is the second encoder 4-7, theta_M6For the rotational angle value of the joint M6, the corresponding encoder is the third encoder 4-8.

As shown in fig. 5-7, the slave manipulator arm comprises a slave end base 201, a link base 202, a slave end link I203, a slave end link II204, an instrument lift base 206, and an instrument mounting device 205. The link base 202 is rotatably mounted at one end of the slave base 201 with its axis of rotation aligned with the direction of gravity. The surgical instrument 207 may be mounted on the instrument mounting device 205.

As shown in fig. 8, the configuration of the slave arm is such that the joint S1 is a rotary joint for moving the link base 202, the slave end link I203, the slave end link II204, and the instrument lifter base 206 in the entire lateral direction, the joint S2 is a parallelogram-shaped pivoting joint that pivots in the pitch direction about the axis (i.e., extends or folds the slave end link I203, the slave end link II204, and the instrument lifter base 206), the joint L3 is a joint for moving the instrument attachment device 205 along the instrument lifter base 206, the joints S1 and S2 determine the position of the distal dead point (point P in fig. 7), and the joint L3 that moves in the vertical direction about the axis of the distal dead point determines the position of point P. As shown in fig. 9 and 10, the surgical instrument adopts the wrist degree of freedom layout of "self-transmission-deflection-end self-transmission", and the joints S4, S5 and S6 of the wrist at the end of the surgical instrument determine the posture of the end of the surgical instrument. Similarly, the slave manipulator end also adopts a D-H parameter method to establish a kinematic model of the slave manipulator, and the homogeneous transformation matrix of the tail end of the slave manipulator in the base coordinate system is obtained according to the homogeneous transformation matrix calculation formulaThe position of the surgical instrument tip in the base coordinate system from the manipulator arm can be derived from the homogeneous transformation matrix. The rotation angle values theta of the three joints of the wrist of the surgical instrument can be calculated according to the following formula_SiAre each theta_S4、θ_S5、θ_S6And represents the posture of the wrist from the end of the manipulator instrument.

θ_Si＝A_Si/μ_Si

Wherein A is_SiRepresents the amount of change, mu, of the joint encoder value_SiThe product of the joint encoder precision, the speed reduction ratio of the speed reducer and the transmission ratio of the mechanical structure.

According to the designed configurations of the main operating arm and the auxiliary operating arm, the three joint axes of the wrist of the surgical instrument and the tail end isomorphic main hand of the surgical instrument are intersected at one point, and the position and the posture decoupling in the kinematic analysis can be realized. By combining the structural characteristics, the invention establishes master-slave mapping with separated pose and isomorphic tail ends. In the position and motion mapping, a position base coordinate system of the master operation arm needs to be mapped into a base coordinate system described by the slave operation arm, and a homogeneous transformation matrix of the slave operation arm position is obtained after mapping as follows:

wherein the content of the first and second substances,is a transformation matrix from the base coordinate system of the master operation arm to the base coordinate system of the slave operation arm,the motion of the main manipulator is mapped into the motion of the manipulator after being reduced according to a certain proportion for the scale factor of the master-slave mapping.The following can also be written:

from the position coordinates of the operating armAccording to a homogeneous transformation matrix of the joints S1, S2 and L3 of the manipulator arm, which is derived from the configuration of the manipulator arm according to a D-H parameter method:

wherein each element in the matrix is a joint 1 to joint 3 parameter (θ)_s1、θ_s2、l_s3) Is described in (1). The following relationships are established according to the above description:

p_sx＝x_s

p_sy＝y_s

p_sz＝z_s

according to the three equations, the unknown parameters theta of the joint S1, the joint S2 and the joint L3 can be obtained_s1、θ_s2、l_s3。

And the posture mapping of the instrument and the tail end of the main hand is realized by adopting a one-to-one mapping mode. Therefore, the calculation formula of the expected angle values of the wrist three joints at the tail end of the operation arm instrument is as follows:

θ_s4＝θ_M4

θ_s5＝θ_M5

θ_s6＝θ_M6

wherein, theta_M4、θ_M5、θ_M6Respectively represent the actual rotation angle values of the three joints of the main operation wrist part. Then the motion controller is used for outputting corresponding numerical values to the slave operation arm, and the purpose of master-slave following is achieved. The invention provides a modeling method for simplifying a kinematics model of a minimally invasive surgery robot according to the concept of master-slave operation arm configuration, namely the concept of separation of postures and isomorphism of tail ends, simplifies the complexity of the whole kinematics calculation process and improves the real-time property of a motion control system.

The above description is only for the purpose of illustrating preferred embodiments of the present invention and is not to be construed as limiting the present invention, and it is apparent to those skilled in the art that various modifications and variations can be made in the present invention.