Incremental master-slave heterogeneous minimally invasive surgery robot system and control algorithm

Through the incremental master-slave heterogeneous minimally invasive surgical robot system and control algorithm, the problem of insufficient surgical accuracy and safety in the existing technology is solved, efficient and accurate master-slave control is achieved, and the difficulty of system design is reduced and the difficulty of system design is improved. Reliability and ease of operation.

CN119033470AInactive Publication Date: 2024-11-29BEIJING INFORMATION SCI & TECH UNIV +1

Patent Information

Application Number
CN202411158904.7
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-22
Publication Date
2024-11-29
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

The existing minimally invasive surgical robot systems have shortcomings in surgical accuracy and safety, and the system design and implementation are difficult to achieve efficient and accurate master-slave control.

Method used

The incremental master-slave heterogeneous minimally invasive surgical robot system and control algorithm, including hardware structure and software structure, collect doctor's hand information through the master hand equipment, filter out physiological jitter, establish a motion mapping relationship, and perform inverse kinematic solution , combined with TWINCAT system and ADS communication technology, realize precise posture control from the hand instrument arm, integrate jitter filtering and safety restriction functions.

Benefits of technology

It improves the accuracy and safety of the operation, optimizes the orderliness and simplicity of the operation, reduces the difficulty of system design and implementation, and ensures the efficiency and reliability of the surgical process.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119033470A_ABST
    Figure CN119033470A_ABST
Patent Text Reader

Abstract

The invention discloses an incremental master-slave heterogeneous minimally invasive surgery robot system and a control algorithm, and belongs to the field of robotics.The incremental master-slave heterogeneous minimally invasive surgery robot system comprises the steps that the position and posture information of the hands of a doctor is collected, jitter is filtered out, and a more accurate control signal is obtained; establishing a motion mapping relation between the master hand equipment and the slave hand instrument arm, and taking a mapped pose as a target pose of the slave hand instrument arm; performing inverse kinematics calculation on the mapped position and attitude data according to the slave manipulator instrument arm configuration, and calculating angle information of each joint of the slave manipulator instrument arm; and a joint decoupling algorithm, back clearance compensation and a polynomial interpolation method are adopted, the actual position of each driving motor is calculated, and accurate pose control of the slave manipulator mechanical arm is achieved. According to the incremental master-slave heterogeneous minimally invasive surgery robot system and the control algorithm, the precision and safety of surgery are improved, the control process is optimized, the orderliness and simplicity of surgery operation are improved, and the difficulty of system design and implementation is reduced.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to an incremental master-slave heterogeneous minimally invasive surgical robot system and a control algorithm. Background Art

[0002] Currently, surgical robot systems have been developed due to the constraints of traditional surgery in terms of workspace limitations and reducing surgeon fatigue. The rise of modern minimally invasive surgery technology has brought revolutionary changes to traditional surgical methods. At the same time, the application of surgical robots is becoming more and more widespread in the field of modern minimally invasive surgery.

[0003] The instrument arm of a minimally invasive surgical robot is a key tool that replaces doctors in performing surgical operations. Its design configuration has a decisive influence on the flexibility of operation and the accuracy of control. These robots generally adopt a master-slave operation mode, in which the operator manipulates the master hand device through the real-time image obtained by the three-dimensional visual feedback system at the master console, and the slave hand instrument arm accurately reproduces the operator's hand movements according to the output of the control system. This master-slave operation mode effectively frees doctors from the physical surgical environment, allowing them to sit in front of the console without having to stand for a long time, and concentrate on the surgical operation, thereby greatly improving the accuracy and reliability of the operation. The master-slave control strategy and method are the core of the research on minimally invasive surgical robot technology, and have extremely high research value and application prospects. Summary of the invention

[0004] The purpose of the present invention is to provide an incremental master-slave heterogeneous minimally invasive surgical robot system and control algorithm, which improves the accuracy and safety of surgery, improves the orderliness and simplicity of surgical operations, and reduces the difficulty of system design and implementation.

[0005] To achieve the above-mentioned purpose, the present invention provides an incremental master-slave heterogeneous minimally invasive surgical robot system, including a hardware structure and a software structure. The hardware structure includes a host computer and a slave computer connected to the host computer, the host computer is connected to a surgical robot, a foot pedal and a camera, the surgical robot includes a master hand device and a slave hand instrument arm, and a drive system is arranged in the slave computer. The drive system includes seven drive units, each drive unit includes several DC motors, a reducer, an encoder and a drive motor; the software structure includes a user layer, a motion planning layer and a motion execution layer.

[0006] Preferably, the lower computer is a TWINCAT system.

[0007] A control algorithm for an incremental master-slave heterogeneous minimally invasive surgical robot system includes four stages:

[0008] a. Collect the position and posture information of the doctor's hand through the main hand device, and filter out the physiological tremor in the doctor's hand movement information to obtain more accurate control signals;

[0009] b. Establishing a motion mapping relationship between the master hand device and the slave hand instrument arm, and using the mapped position and posture as the target position and posture of the slave hand instrument arm;

[0010] c. Perform inverse kinematics calculation on the mapped position and posture data according to the configuration of the slave arm to calculate the angle information of each joint of the slave arm;

[0011] d. Based on the TWINCAT system, the processed joint angle information is received through ADS communication technology, and the joint decoupling algorithm, backlash compensation and polynomial interpolation method are used to calculate the actual position of each drive motor, and then control the movement of the drive motor to achieve precise posture control of the slave arm.

[0012] Preferably, a Butterworth filter is used to filter out physiological jitter in the doctor's hand movement information.

[0013] Preferably, the motion mapping relationship between the master hand device and the slave hand instrument arm is established as follows:

[0014] By determining the dynamic zero point of the master hand device, the position P of its dynamic zero point is obtained. M0 , and at the same time obtain the position P of the dynamic zero point of the hand instrument arm S0 When the doctor operates the main hand device to move to a certain moment, the position P of the main hand device at that moment is obtained MC and the position P of the hand arm SC , when the main hand device moves to the current position P MC Calculate the master hand device relative to the dynamic zero position P M0 The position increment, that is, (P MC -P M0 ), multiply the position increment by the proportional factor k, and obtain the dynamic zero point P of the slave instrument arm. S0 The position increment is applied to calculate the current target position P at the end of the hand arm. SC ; Then the position mapping relationship between the master hand device and the slave hand instrument arm is as follows:

[0015] P SC =P S0 +k(P MC -P M0 )

[0016] The mapping relationship between the end-of-arm posture of the slave hand and the posture increment of the master hand device is as follows:

[0017] R SC = Rotz(ΔθMZ )*Roty(Δθ MY )*Rotx(Δθ MX )*R S0

[0018] R S0 is the dynamic zero point of the end posture of the slave robot arm, and this dynamic zero point is also selected. Rotz(),Roty(),Rotx() respectively represent the 3×3 rotation matrix generated by the rotation of the slave robot arm around the Z, Y, and X axes of its end tool coordinate system; Δθ MZ ,Δθ MY and Δθ MX are the rotation increments of the wrist joints of the main hand device; R SC Indicates that the receiving robot arm is at the dynamic zero point R S0 On this basis, the current target position and posture are obtained through the posture mapping relationship.

[0019] Preferably, sequential quadratic programming is used to solve the inverse kinematics:

[0020] The joint limits of the slave arm are taken as constraints, and the objective function is designed as the target pose matrix T d With the current actual pose matrix T c The sum of the absolute values ​​of the position error and attitude error between is as follows:

[0021]

[0022] Among them, err orientation =err r +err p +err y , err position =err x +err y +err z .

[0023] Preferably, the specific contents of realizing precise posture control of the slave instrument arm are:

[0024] The joint motion angle obtained by inverse kinematics solution is processed by the multi-joint decoupling algorithm to obtain the target motion angle of the drive motor. It is judged whether the joint is moving in the reverse direction. If it is moving in the reverse direction, backlash compensation is performed; the compensated angle is output to the motor driver in the drive motor.

[0025] Therefore, the present invention adopts the above-mentioned incremental master-slave heterogeneous minimally invasive surgical robot system and control algorithm, which has the following beneficial effects:

[0026] (1) A motion mapping relationship between the master hand device and the slave hand instrument arm is established, allowing the doctor to accurately and efficiently control the slave hand instrument arm to perform a series of surgical actions by operating the master hand device; it also integrates the master hand jitter filtering and master hand safety limit functions, further improving the accuracy and safety of the surgery;

[0027] (2) The control process was optimized, the orderliness and simplicity of surgical operations were improved, and the difficulty of system design and implementation was reduced.

[0028] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 A schematic diagram of the hardware structure of a control system according to an embodiment of the present invention;

[0030] Figure 2 A schematic diagram of the software structure of a control system according to an embodiment of the present invention;

[0031] Figure 3 This is a master hand jitter filter diagram according to an embodiment of the present invention. DETAILED DESCRIPTION

[0032] Example

[0033] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0034] Reference Figure 1 and Figure 2 The present invention discloses an incremental master-slave heterogeneous minimally invasive surgical robot system, including a hardware structure and a software structure. The hardware structure includes a host computer and a slave computer connected to the host computer. The host computer is connected to a surgical robot, a foot pedal and a camera. The surgical robot includes a master hand device and a slave hand instrument arm. A drive system is arranged in the slave computer. The drive system includes seven drive units, each of which includes several DC motors, a reducer, an encoder and a drive motor; the software structure includes a user layer, a motion planning layer and a motion execution layer.

[0035] The development environment of the host computer is completed in a PC based on the Linux operating system, and ROS (Robot Operating System) is integrated into it. The rich features of ROS can help robot projects to develop quickly and realize algorithm verification. The development environment of the host computer is Ubuntu18.04, ROS-Melodic. The host computer is responsible for some upper-level control algorithms and human-computer interaction interfaces, connecting the Omega.7 master hand device, pedals and cameras, and realizing master-slave consistency control, master-slave safety restrictions, motion field feedback, inverse kinematics solution, ADS (The Automation Device Specification) communication and other functions.

[0036] The lower computer is the driving system of the surgical robot. It consists of a set of drive units, including a brushless DC motor, a reducer, an encoder and a driver, to complete the motion control of a single motor. The entire drive system has seven drive units, corresponding to the seven degrees of freedom of the actuator arm. In order to achieve precise control of the multi-axis motion of the surgical robot, this embodiment uses the EtherCAT protocol as the field bus.

[0037] In the lower computer part, this embodiment chooses to carry out development in a PC based on Windows operating system, and the development environment uses TwinCAT3.

[0038] The ADS communication protocol based on TCP / IP is used for data communication between the upper and lower computers. The ADS communication protocol was proposed by Beckhoff based on the TCP / IP protocol, which provides efficient, stable and reliable data transmission, and can achieve high-speed data transmission and low-latency response.

[0039] like Figure 2 As shown in the software structure diagram, each software layer is an independent process. There are multiple threads in the software layer to perform their respective tasks. Multi-threading improves the utilization of CPU resources.

[0040] In ROS, processes exist in the form of nodes, and data communication between different nodes can be achieved through topics.

[0041] The control system software structure is divided into user layer, motion planning layer and motion execution layer. The user layer is responsible for the human-machine interaction between the doctor and the surgical robot. The motion planning layer is responsible for completing the master-slave consistency movement of the master hand device and the slave hand instrument arm. The motion execution layer is responsible for controlling each drive motor of the slave hand instrument arm to complete the command movement.

[0042] The software development of the user layer and motion planning layer is completed in the host PC, and the software development of the motion execution layer is completed in the slave PC.

[0043] The user layer and motion planning layer are developed in the ROS-based host PC. ROS supports a thread library based on the C++11 standard and uses the std::thread thread library to create and manage threads. The user layer and motion planning layer are two different software layers, which are specifically manifested as two different nodes in ROS and communicate through the ROS Master. When a node starts, it registers its name and topic, service, parameter and other information with the ROS Master, and other nodes can find and connect to the node through the ROSMaster.

[0044] The user layer nodes mainly include the device main thread, pedal detection thread, camera shooting thread, master hand force balance thread, master hand device acquisition thread and master-slave safety restriction thread.

[0045] The device main thread is the core thread of the user layer. The device main thread first completes the connection of the main hand device, pedal and camera, and then creates the pedal detection thread, camera shooting thread, main hand device acquisition thread, main hand force balance thread and master-slave safety restriction thread based on the std::thread thread library provided by C++11, and finally calls the join() function to make the above threads enter the blocking state to ensure that all sub-threads can be executed.

[0046] The pedal detection thread is responsible for real-time detection of whether the pedal is pressed. When the doctor moves the master hand device to the boundary of the workspace, or the operation posture is uncomfortable and needs to be readjusted, the doctor can disconnect the master-slave control by stepping on the corresponding pedal and readjust the posture of the master hand device. The pedal can also be used to reselect the scale factor of the master-slave control and execute operations such as master-slave safety area restrictions.

[0047] The main hand device force balance thread is responsible for offsetting the gravity generated by the wrist of the main hand device. By calling the dhdSetForceAndTorqueAndGripperForce() function in the main hand device SDK, the three motors that control the position of the main hand device generate relevant torques, thereby reducing the doctor's hand operation pressure and improving overall comfort.

[0048] The master-slave safety restriction thread is responsible for restricting the master hand device within the specified safety area. The master-slave safety restriction mode is selected through the foot pedal. In this mode, the reaction force is calculated through the spring damping model according to the positional relationship between the current position of the master hand device and the boundary of the set safety area. The dhdSetForce() function is called to apply the force in the opposite direction of the master hand movement, simulating the effect of spring damping, and rebounding the master hand device so that the master hand device can only move within the specified safety area.

[0049] The main hand device acquisition thread is responsible for obtaining the doctor's hand motion information in real time. The position and posture are obtained through the dhdGetPositionAndOrientationRad() function provided by the main hand device SDK, and the obtained position and posture are saved as a float64 type array. The linear velocity is obtained through the dhdGetLinearVelocity() function, and the gripper angle is obtained through the dhdGetGripperAngleRad() function. The above main hand device motion information is packaged into a ROS custom message surgical_robot_msgs::Omega7_pose, and published to the topic / Omega7_pose through the publisher.

[0050] The motion planning layer is running as a ROS node. There is only one main thread in the motion planning layer. A subscriber masterPoseSub is created in the main thread. The subscriber subscribes to the / Omega7_pose topic published by the user layer. The / Omega7_pose topic is updated in real time with a sampling frequency of 100Hz. Whenever there is data update on the topic, the masterPoseSub subscriber callback function is entered. In the callback function, the desired position of the slave arm is obtained through the master-slave control method, and then the desired angles of each joint of the slave arm are obtained through the nonlinear inverse kinematics solution. The angles are saved as an array in the float64[] type and sent to the motion execution layer of the lower computer through the ADS communication protocol.

[0051] The motion execution layer is implemented in Windows-based TwinCAT3. First, make hardware connections, place the driver's XML configuration file in the Config\Io\EtherCAT directory of TwinCAT3, perform hardware scanning, configure the NC axis, and then complete the hardware connection. TwinCAT3 will obtain the expected joint angle obtained by the inverse kinematics solution in the host computer every 10ms, calculate the expected angle of the drive motor through the wire drive model, and control the motor to move to the specified angle through the NC axis to complete the motion control of the slave arm.

[0052] A control algorithm for an incremental master-slave heterogeneous minimally invasive surgical robot system includes four stages:

[0053] a. Collect the position and posture information of the doctor's hand through the main hand device, and filter out the physiological tremor in the doctor's hand movement information to obtain more accurate control signals;

[0054] b. Establishing a motion mapping relationship between the master hand device and the slave hand instrument arm, and using the mapped position and posture as the target position and posture of the slave hand instrument arm;

[0055] c. Perform inverse kinematics calculation on the mapped position and posture data according to the configuration of the slave arm to calculate the angle information of each joint of the slave arm;

[0056] d. Based on the TWINCAT system, the processed joint angle information is received through ADS communication technology, and the joint decoupling algorithm, backlash compensation and polynomial interpolation method are used to calculate the actual position of each drive motor, and then control the movement of the drive motor to achieve precise posture control of the slave arm.

[0057] The Butterworth filter is used to filter out the physiological tremor in the doctor's hand movement information. The Butterworth filter is an efficient filter that can effectively filter out signals within a specific frequency range and retain useful movement information, thereby improving the quality of the control signal.

[0058] like Figure 3 The master hand jitter filter diagram is shown below. The sampling frequency of the master hand device is 100Hz. It is necessary to filter out the hand jitter signal of 8-12Hz. The passband range of the filter is set to all frequency ranges except 8-12Hz. This is achieved using a second-order band-stop filter. The transfer function of the Butterworth band-stop filter can be expressed as the product of two first-order low-pass filters and two first-order high-pass filters, as shown below:

[0059]

[0060] K is the gain factor, w0 is the center frequency, s0 is the zero point, and Q0 is the quality factor. For an n-order band-stop filter, it can be realized by decomposing n into two integers p and q, where p and q are the orders of two first-order low-pass filters. 2 Replace with In the form of, and into the bilinear transformation formula, we can get the transfer function in the discrete time domain:

[0061]

[0062] In the formula, b_0, b_1, ..., b_n and a_1, a_2, ..., a_n are the coefficients of the filter. In Matlab, the "butter" function can be used to design a Butterworth second-order band-stop filter, and the coefficients are calculated as a = [1, -1.447972597, 0.775679511], b = [0.887839755, -1.447972597, 0.8878397555] to perform the main hand shake filter test.

[0063] The original position data and filtered position data of the master hand device are published to the corresponding topics respectively. The filtering effect of the position signal is viewed in real time through the Rqt drawing tool provided by ROS. The solid line represents the original position signal of the master hand device, and the dotted line represents the filtered position signal of the master hand device. The dotted line is more stable than the solid line, and the filtering effect is obvious.

[0064] The motion mapping relationship between the master hand device and the slave hand instrument arm is established as follows:

[0065] By determining the dynamic zero point of the master hand device, the position P of its dynamic zero point is obtained. M0 , and at the same time obtain the position P of the dynamic zero point of the hand instrument arm S0 When the doctor operates the main hand device to move to a certain moment, the position P of the main hand device at that moment is obtained MC and the position P of the hand arm SC , when the main hand device moves to the current position P MC Calculate the master hand device relative to the dynamic zero position P M0 The position increment, that is, (P MC -P M0 ), multiply the position increment by the proportional factor k, and obtain the dynamic zero point P of the slave instrument arm. S0 The position increment is applied to calculate the current target position P at the end of the hand arm. SC ; Then the position mapping relationship between the master hand device and the slave hand instrument arm is as follows:

[0066] P SC =P S0 +k(P MC -P M0 )

[0067] The mapping relationship between the end-of-arm posture of the slave hand and the posture increment of the master hand device is as follows:

[0068] R SC = Rotz(Δθ MZ )*Roty(Δθ MY )*Rotx(Δθ MX )*R S0

[0069] R S0 is the dynamic zero point of the end posture of the slave robot arm, and this dynamic zero point is also selected. Rotz(),Roty(),Rotx() respectively represent the 3×3 rotation matrix generated by the rotation of the slave robot arm around the Z, Y, and X axes of its end tool coordinate system; Δθ MZ ,Δθ MY and Δθ MX are the rotation increments of the wrist joints of the main hand device; RSC Indicates that the receiving robot arm is at the dynamic zero point R S0 On this basis, the current target position and posture are obtained through the posture mapping relationship.

[0070] Use sequential quadratic programming to solve inverse kinematics:

[0071] In terms of kinematics solution, the present invention adopts a nonlinear optimization constrained numerical inverse solution method, namely Sequential Quadratic Programming (SQP), to solve inverse kinematics. SQP is an efficient algorithm suitable for solving optimization problems that contain both equality and inequality constraints, and is particularly suitable for the precise calculation of the angles of each joint of the robot arm in the present invention. The SQP algorithm is used to solve the inverse kinematics problem of the surgical robot arm, and the joint limits of the slave instrument arm are used as constraints. The objective function is designed as the target posture matrix T d With the current actual pose matrix T c The sum of the absolute values ​​of the position error and attitude error between is as follows:

[0072]

[0073] Among them, err orientation =err r +err p +err y , err position =err x +err y +err z , the angle limits of each joint are shown in Table 1:

[0074] Table 1 Angle limits of each joint

[0075]

[0076]

[0077] The specific contents of realizing precise posture control of the slave instrument arm are as follows:

[0078] The joint decoupling algorithm is used to process joint motion, and backlash compensation is performed for possible reverse motion to ensure the smoothness and accuracy of the robot arm's motion. The joint motion angle obtained by inverse kinematics is processed by the multi-joint decoupling algorithm to obtain the motor target motion angle, and then it is judged whether the joint is in reverse motion. If it is reverse motion, backlash compensation is performed. Finally, the compensated angle is output to the motor driver to control the motor rotation, and the precise posture control of the surgical robot arm can be completed.

[0079] Therefore, the present invention adopts the above-mentioned incremental master-slave heterogeneous minimally invasive surgical robot system and control algorithm to establish a motion mapping relationship between the master hand device and the slave hand instrument arm, allowing the doctor to accurately and efficiently control the slave hand instrument arm to perform a series of surgical actions by operating the master hand device. In addition, the master hand jitter filtering and master hand safety limitation functions are integrated to further improve the accuracy and safety of the operation. Not only does it optimize the control process, improve the orderliness and simplicity of the surgical operation, but it also reduces the difficulty of system design and implementation. It ensures high accuracy and high efficiency during the operation, making the minimally invasive surgical robot system more reliable and easy to operate.

[0080] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. An incremental master-slave heterogeneous minimally invasive surgical robot system, including a hardware structure and a software structure, Features: The hardware structure includes a host computer and a slave computer connected to the host computer. The host computer is connected to a surgical robot, a foot pedal and a camera. The surgical robot includes a master hand device and a slave hand instrument arm. A drive system is arranged in the slave computer. The drive system includes seven drive units. Each drive unit includes several DC motors, reducers, encoders and drive motors. The software structure includes a user layer, a motion planning layer and a motion execution layer.

2. The incremental master-slave heterogeneous minimally invasive surgical robot system according to claim 1, Features: The lower computer is TWINCAT system.

3. A control algorithm for an incremental master-slave heterogeneous minimally invasive surgical robot system as claimed in any one of claims 1 to 2, It is characterized in that There are four stages: a. Collect the position and posture information of the doctor's hand through the main hand device, and filter out the physiological tremor in the doctor's hand movement information to obtain more accurate control signals; b. Establishing a motion mapping relationship between the master hand device and the slave hand instrument arm, and using the mapped position and posture as the target position and posture of the slave hand instrument arm; c. Perform inverse kinematics calculation on the mapped position and posture data according to the configuration of the slave arm to calculate the angle information of each joint of the slave arm; d. Based on the TWINCAT system, the processed joint angle information is received through ADS communication technology, and the joint decoupling algorithm, backlash compensation and polynomial interpolation method are used to calculate the actual position of each drive motor, and then control the movement of the drive motor to achieve precise posture control of the slave arm.

4. The control algorithm of the incremental master-slave heterogeneous minimally invasive surgical robot system according to claim 3, Features: The Butterworth filter is used to filter out the physiological tremor in the doctor's hand movement information.

5. The control algorithm of the incremental master-slave heterogeneous minimally invasive surgical robot system according to claim 4, It is characterized in that The motion mapping relationship between the master hand device and the slave hand instrument arm is established as follows: By determining the dynamic zero point of the master hand device, the position P of its dynamic zero point is obtained. M0 , and at the same time obtain the position P of the dynamic zero point of the hand instrument arm S0 When the doctor operates the main hand device to move to a certain moment, the position P of the main hand device at that moment is obtained MC and the position P of the hand arm SC , when the main hand device moves to the current position P MC Calculate the master hand device relative to the dynamic zero position P M0 The position increment is P MC -P M0 The position increment is multiplied by the proportional factor k, and the dynamic zero point P of the slave instrument arm is S0 The position increment is applied to calculate the current target position P at the end of the hand arm. SC ; Then the position mapping relationship between the master hand device and the slave hand instrument arm is as follows: P SC =P S0 +k(P MC -P M0 ) The mapping relationship between the end-of-arm posture of the slave hand and the posture increment of the master hand device is as follows: R SC =Rotz(Δθ MZ )*Roty(Δθ MY )*Rotx(Δθ MX )*R S0 R S0 is the dynamic zero point of the selected end-of-arm posture of the slave instrument; Rotz(), Roty(), Rotx() respectively represent the 3×3 rotation matrix generated by the rotation of the slave instrument arm around the Z, Y, and X axes of its end-of-arm tool coordinate system; Δθ MZ ,Δθ MY and Δθ MX are the rotation increments of the wrist joints of the main hand device; R SC Indicates that the slave instrument arm is at the dynamic zero point R S0 On this basis, the current target position and posture are obtained through the posture mapping relationship.

6. The control algorithm of the incremental master-slave heterogeneous minimally invasive surgical robot system according to claim 5, It is characterized in that Use sequential quadratic programming to solve inverse kinematics: The joint limits of the slave arm are taken as constraints, and the objective function is designed as the target pose matrix T d With the current actual pose matrix T c The sum of the absolute values ​​of the position error and attitude error between is as follows: where err orientation = err r + err p + err y and err position = err x + err y + err z .

7. The control algorithm of the incremental master-slave heterogeneous minimally invasive surgical robot system according to claim 6, It is characterized in that The specific contents of realizing precise posture control of the slave instrument arm are as follows: The joint motion angle obtained by inverse kinematics solution is processed by the multi-joint decoupling algorithm to obtain the target motion angle of the drive motor, and judge whether the joint is in reverse motion. If it is reverse motion, backlash compensation is performed; The compensated angle is output to the motor driver in the drive motor.

Citation Information

Patent Citations

  • Inverse kinematics solving system of robot

    CN107030698A

  • Precise master-slave control system and method for puncture surgery robot

    CN117017507A

  • Hybrid workspace mapping method for master-slave heterogeneous teleoperation robot

    CN117207158A

  • Incremental master-slave mapping method of master-slave heterogeneous teleoperation system

    CN117731404A

  • Inverse kinematics solution system for use with robots

    US20200139541A1

Cited By

  • Data mapping method and system based on camera assembly

    CN121353818A

  • Control method for low-delay teleoperation of surgical robot

    CN121691300A