Surgical robot collaborative planning and collision avoidance system

Abstract

The invention discloses a surgical robot collaborative planning and collision avoidance system, and aims to improve the safety and accuracy of a single-port endoscopic surgery. The system comprises a surgical end effector module, a system modeling module, a data acquisition module, a central processing module, a path planning module, a dynamic adjustment module and a collaborative mechanism module. Through cooperative work of multiple modules, efficient operation of surgical instruments is achieved, and collision is avoided. The path planning module proposes a novel polynomial curve fitting method to improve the smoothness of the path, and plans an optimal path for each actuator in combination with an actuator kinematic model and space geometric constraints. The dynamic adjusting module automatically adjusts the speed and the direction of the actuator based on real-time sensing data, and the safety and the adaptability of a path are ensured. The cooperation mechanism module coordinates actions of the multiple actuators through the central processing module, and time and space synchronization among the actuators is ensured. The safety, accuracy and adaptability of the surgery are remarkably improved, and reliable technical support is provided for the minimally invasive surgery.

Description

TECHNICAL FIELD

[0001] The present application relates to a surgical robot cooperative planning and collision avoidance system, belonging to the technical field of robots, in particular to the technical field of single-hole laparoscopic surgical robots. BACKGROUND

[0002] The single-hole laparoscopic surgical robot is an advanced minimally invasive surgical device that realizes precise operation of the surgical end effector through a single surgical channel. However, in the surgical process, multiple surgical end effectors enter the body through the same channel, and there is a risk of collision between instruments. Therefore, how to realize the cooperative planning and collision avoidance of the surgical end effector is a problem to be solved in the technical field of single-hole laparoscopic surgical robots.

[0003] The traditional method usually adopts static collision envelope and simple threshold judgment, and in the cooperative task of multiple robots, the response ability to millisecond-level dynamic changes has room for improvement; when the instruments enter the same narrow working area at the same time, the system usually slows down or pauses to ensure safety, and the surgical rhythm is inevitably affected to a certain extent, so the traditional method still has room for improvement in collision detection, dynamic cooperation, trajectory smoothing, and dynamics model. SUMMARY

[0004] The present application solves the technical problem of overcoming the shortcomings of the prior art and providing a surgical robot cooperative planning and collision avoidance system that ensures the safety and precision of surgical end effectors in single-hole laparoscopic surgery through optimized path planning and real-time collision avoidance mechanism.

[0005] The technical solution of the present application is a surgical robot cooperative planning and collision avoidance system, comprising:

[0006] A surgical end effector module comprising a plurality of surgical end effectors and an endoscope effector, each surgical end effector being provided with a position and gripping force sensor for real-time monitoring of the motion state and gripping force of the instrument;

[0007] A system modeling module for providing kinematic and dynamic models and corresponding physical constraints for the data acquisition module and the path planning module;

[0008] A data acquisition module for real-time acquisition of state data of each sensor of the surgical end effector and visual information of the surgical area where the surgical end effector module is located;

[0009] A central processing module that generates surgical environment information and position and attitude information of the surgical end effector according to the kinematic and dynamic models and the received state data and visual information, combines a pre-set kinematic solving method and an image processing method, sends the surgical environment information to the path planning module and the cooperative mechanism module, and sends the position and attitude information to the dynamic adjustment module and the cooperative mechanism module.

[0010] The path planning module, based on the received surgical environment information, combined with the kinematic and dynamic models and the corresponding physical constraints, plans the optimal path for each surgical end effector, and sends it to the dynamic adjustment module after smoothing.

[0011] The dynamic adjustment module dynamically adjusts the speed and direction of the surgical end effector based on the received smooth optimal path and position and posture information to avoid collisions, and sends the adjustment results to the coordination mechanism module.

[0012] The collaborative mechanism module, based on the received surgical environment information, position and posture information, and adjustment results, and in conjunction with surgical requirements, completes the collaborative operation between the surgical end effector and the endoscope actuator under external command control, ensuring the temporal and spatial synchronization between the surgical end effector and the endoscope actuator, thereby achieving the surgical task.

[0013] Furthermore, the surgical end effector and the endoscopic actuator have multiple rotational degrees of freedom and at least one overall translational degree of freedom to achieve spatial operation, and different surgical instruments are installed at the end of the surgical end effector according to the surgical requirements.

[0014] Furthermore, the physical constraints include preventing the two surgical end effectors from colliding; the method for determining whether the two surgical end effectors collide includes:

[0015] (1) Extract the coordinates of all key points on the first surgical end effector and the coordinates of all key points on the second surgical end effector; wherein, the i-th key point A on the first surgical end effector i The coordinates are (x ai ,y ai ,z ai ), the i-th key point B on the second surgical end effector i The coordinates are (x bi ,y bi ,z bi );

[0016] (2) Calculate key point A i Distances to all key points on the second surgical end effector; Key point A i With the j-th key point B on the second surgical end effector j distance d ai_bj for:

[0017]

[0018] (3) All calculated distances d ai_bj Sort the data, find the two smallest distances, and the distance to key point A. iTwo recent key points B l B m ;

[0019] (4) Solve for key point A i With key point B l Line segment B formed by two adjacent key points l B l and line segment B l B l+1 The distance between them, and key point A i With key point B m Line segment B formed by two adjacent key points m B m and line segment B m B m+1 The distance between them; key point A i With line segment B l B l The distance between them is:

[0020]

[0021] The denominator is vector B. l B l The modulus, with the numerator being vector A. i B l With vector A i B l The modulus of the cross product;

[0022] (5) Based on key point A i The distance d between the four line segments mentioned above ai_bl_bl d ai_bl_bl+1 d ai_bm_bm d ai_bm_bm+1 Determine whether the two surgical end effectors collide.

[0023] Furthermore, determine key point A. i The distance d between the four line segments mentioned above ai_bl_bl d ai_bl_bl+1 d ai_bm_bm d ai_bm_bm+1 Is it less than the expected value d? min ;

[0024] If there exists a distance less than or equal to the expected value d min If so, it is considered that the two surgical end effectors collided;

[0025] If all distances are greater than the expected value d min If so, it is assumed that the two surgical end effectors did not collide.

[0026] Furthermore, the dynamic model includes:

[0027]

[0028] wherein τ is a joint torque matrix; τ f is a friction torque matrix related to the friction force F f = μN; μ is a friction coefficient; N is a normal force, i.e. a force normal to the contact surface; θ is a joint angle matrix; is a joint angular velocity matrix; is a joint angular acceleration matrix; M(θ) is an inertia matrix; is a centripetal and Coriolis force matrix; G(θ) is a gravity matrix related to the gravity force F g = mg; m is the mass of the surgical end effector; g is the gravitational acceleration; P op is a force matrix related to the manipulation force F op .

[0029] Further, the manipulation force F op is:

[0030]

[0031] wherein β, are the angle, angular velocity, angular acceleration of the manipulation end manipulator; x end , v end , a end are the position, velocity, acceleration of the manipulation end manipulator end point, which are calculated by the joint angle β, angular velocity angular acceleration ; K u , B u , M u are the stiffness, damping, inertia matrix of the manipulation end manipulator.

[0032] Further, the smoothing processing comprises:

[0033] (1) determining the coordinates (x pi , y pi , z pi ) of the inflection point P i , and the coordinates (x pi , y pi , z pi ) and (x pi+1 , y pi+1 , z pi+1 ) of the two adjacent points P i and P i+1 , and calculating the line segment P i P i and the line segment P i Pi+1 The angle β formed i :

[0034]

[0035] (2) Calculate the smoothness of the smooth curve λ i ;

[0036] (3) Calculate the starting point of the smooth path and the end point of the smooth path

[0037]

[0038] Through the above formula and the coordinates of P i , P i , P i+1 , the coordinates of P and P are respectively represented as P and P

[0039] (4) Obtain the function corresponding to the smooth path by combining the constraint conditions of the smooth path.

[0040] Further, the constraint conditions of the smooth path include: the starting point is P , the end point is P , the starting direction is the same as P i P i direction, and the ending direction is the same as P i P i+1 direction.

[0041] Further, the smoothness of the smooth curve is K , and K g is a constant parameter.

[0042] Further, the function S i (t) corresponding to the smooth path is

[0043]

[0044] The advantages of the present application compared with the prior art are:

[0045] (1) High safety: through the real-time collision avoidance mechanism, the collision between the surgical end effectors is effectively avoided, and the cooperative mechanism module coordinates the actions of multiple surgical end effectors through the fuzzy logic controller, ensures the time and space synchronization between the instruments, and avoids the collision risk caused by the incoordination between the instruments.

[0046] (2) Good smoothness: The path smoothing module removes redundant nodes and proposes a new type of polynomial curve fitting method to improve the smoothness of the path, thereby reducing the sudden changes and wear and tear of the surgical end effector during movement, prolonging the service life of the instrument, and improving the smoothness of the surgical operation.

[0047] (3) Good precision: The kinematics and dynamics models of the system modeling module provide accurate mathematical descriptions and physical constraints for path planning and dynamic adjustment, further improving the precision of the surgical operation. High-precision position and attitude sensors and force sensors monitor the motion state and clamping force of the instrument in real time, providing accurate feedback for the operation to help the operator complete the task more accurately.

[0048] (4) High efficiency: The central processing module uses a high-performance computing platform, supports multi-threading and distributed computing, significantly improves data processing speed and system response capability, and ensures real-time and high efficiency of the surgical operation. BRIEF DESCRIPTION OF DRAWINGS

[0049] Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the preferred embodiments. The drawings are for purposes of illustration only and are not considered a limitation of the present application. Moreover, like reference numerals are used to designate identical components throughout the specification. In the drawings:

[0050] Figure 1 is a flowchart of the system of the present application;

[0051] Figure 2 is the ith inflection point P i is the corresponding smooth path (β i = 45°);

[0052] Figure 3 is the ith inflection point P i is the corresponding smooth path (β i = 135°). DETAILED DESCRIPTION

[0053] In order to better understand the above technical solutions, the following will describe the technical solutions of the present application in detail through the drawings and specific embodiments. It should be understood that the embodiments and specific features in the embodiments are detailed descriptions of the technical solutions of the present application, and are not limitations of the technical solutions of the present application. In the case of no conflict, the technical features in the embodiments and the embodiments can be combined with each other.

[0054] The following will further describe a surgical robot cooperative planning and collision avoidance system provided by the embodiments of the present application in detail in combination with the drawings of the specification. The specific implementation manner can include:

[0055] The surgical end effector module comprises a plurality of surgical end effectors and endoscope effectors, each of which is provided with a position and clamping force sensor for monitoring the movement state and clamping force of the instrument in real time;

[0056] The system modeling module is used for providing kinematic models and dynamic models and corresponding physical constraints for the data acquisition module and the path planning module;

[0057] The data acquisition module is used for collecting state data of each sensor of the surgical end effector and visual information of a surgical area where the surgical end effector module is located in real time;

[0058] The central processing module generates surgical environment information and position and attitude information of the surgical end effector according to the kinematic models and the dynamic models and the received state data and visual information, combines a preset kinematic solving method and an image processing method, sends the surgical environment information to the path planning module and the cooperative mechanism module, and sends the position and attitude information to the dynamic adjustment module and the cooperative mechanism module;

[0059] The path planning module plans an optimal path for each surgical end effector according to the received surgical environment information, combines the kinematic models and the dynamic models and corresponding physical constraints, and sends the smoothed optimal path to the dynamic adjustment module after smoothing processing;

[0060] The dynamic adjustment module dynamically adjusts the speed and direction of the surgical end effector according to the received smoothed optimal path and position and attitude information, avoids collision, and sends the adjustment result to the cooperative mechanism module;

[0061] The cooperative mechanism module combines surgical requirements according to the received surgical environment information, position and attitude information and adjustment result, and completes the cooperative operation between the surgical end effectors and the endoscope effectors under the control of external instructions, ensures the time and space synchronization between the surgical end effectors and the endoscope effectors, and thus realizes the surgical task.

[0062] In the scheme provided in the embodiment of the application, as Figure 1As shown, the system includes a surgical end effector module, a system modeling module, a data acquisition module, a central processing module, a path planning module, a dynamic adjustment module, and a coordination mechanism module. The surgical end effector module enters the operating environment through a single-hole channel, and each surgical end effector is equipped with a position and attitude sensor for real-time monitoring of the motion state of the instrument. The system modeling module constructs a kinematic model and a dynamic model for the surgical end effector in the surgical end effector module. The data acquisition module is used to acquire the position and attitude data of the surgical end effector and transmit it to the central processing module. The central processing module receives sensor data and master-end transmission data, monitors the position and attitude of the surgical end effector in real time, and coordinates the work of each module. The path planning module uses an improved RRT algorithm, combined with the kinematic model of the surgical end effector and the geometric constraints of the surgical space, to plan the optimal path for each surgical end effector. The dynamic adjustment module uses a dynamic collision avoidance strategy based on real-time sensor data to automatically adjust the speed and direction of the instrument when potential collisions are detected. The coordination mechanism module coordinates the actions of multiple surgical end effectors through the central processing module to ensure time and space synchronization between surgical end effectors. The specific implementation of each module is described in detail below:

[0063] The surgical end effector module is the core part of the single-hole laparoscopic surgical robot system, consisting of N (N is usually equal to 2 or 3) surgical end effectors and 1 endoscope effector. Each surgical end effector has highly flexible motion capabilities, with multiple rotational degrees of freedom and at least one overall translational degree of freedom, enabling complex spatial operations. The end of the surgical end effector can be equipped with different surgical instruments such as scissors, forceps, and electrocoagulation hooks according to surgical needs to meet the needs of various surgical operations. The end of the endoscope effector is equipped with a high-definition endoscope for real-time observation of the internal conditions of the surgical area, providing clear visual support for surgical operations. Each surgical end effector is equipped with a high-precision position and attitude sensor that monitors the motion state of the instrument in real time, and the surgical instrument is equipped with a force sensor that monitors the size of the clamping force in real time.

[0064] Sensor data is transmitted to the data acquisition module through wireless or wired means. Wireless transmission uses low-latency, high-bandwidth wireless communication protocols such as 5G or Wi-Fi 6 to ensure real-time and stability of data transmission. Wired transmission is achieved through high-speed data cables, suitable for surgical scenarios with higher requirements for data transmission stability and anti-interference capability.

[0065] The system modeling module includes a kinematic model and a dynamic model, which provide accurate mathematical descriptions and physical constraints for the path planning module and dynamic adjustment module of the surgical end effector, ensuring that the surgical end effector can follow its physical characteristics and motion capabilities when planning the path, thereby achieving safe and efficient surgical operations.

[0066] Kinematic model. The kinematic model of the surgical end-effector is precisely modeled according to its structure and function. The kinematic model mainly describes the motion relationship of the surgical end-effector in space, including the following key parts: degrees of freedom and motion range, motion description of the surgical end-effector, motion constraint relationship, and application of the kinematic model.

[0067] Degrees of freedom and motion range. Each surgical end-effector has multiple rotational degrees of freedom and at least one overall translational degree of freedom, and the motion position, motion velocity, and acceleration of each degree of freedom and other parameters are precisely modeled to avoid damage or collision of the mechanical structure.

[0068] The constraint ranges of various parameters are as follows:

[0069]

[0070] where θi is the angle of the i-th rotational degree of freedom, θimin and θimax are the minimum and maximum values of the angle of the i-th rotational degree of freedom, m is the position of the overall translational degree of freedom, mmin and mmax are the minimum and maximum values of the position of the overall translational degree of freedom; i i,min i,max min max

[0071] Motion description of the surgical end-effector. The kinematic model describes the position and attitude of the surgical end-effector through homogeneous transformation matrices and Jacobian matrices. The homogeneous transformation matrix is used to describe the transformation between coordinate systems, and the Jacobian matrix is used to describe the relationship between the linear and angular velocities of the surgical end-effector and the joint velocities.

[0072] ​​​​​​​​​​​​​​​​​Motion constraints. The model considers the motion constraints between surgical end effectors. The movement of the surgical end effector within the single-hole channel is spatially restricted to avoid collisions with other surgical end effectors.

[0073] The criteria for determining whether two surgical end effectors collide are as follows:

[0074] (1) Extract the coordinates of all key points on the first surgical end effector and the coordinates of all key points on the second surgical end effector. Specifically, the coordinates of the i-th key point A on the first surgical end effector are... i The coordinates are (x ai ,y ai ,z ai ), the i-th key point B on the second surgical end effector i The coordinates are (x bi ,y bi ,z bi ).

[0075] (2) The key point A is obtained by using the Euclidean distance formula. i Distances to all key points on the second surgical end effector. Key point A i With the j-th key point B on the second surgical end effector j distance d ai_bj The solution formula is as follows:

[0076]

[0077] (3) All calculated distances d ai_bj Sort the data, find the two smallest distances, and the distance to key point A. i Two recent key points B l B m .

[0078] (4) Solve for key point A i With key point B l Line segment B formed by two adjacent key points l B l and line segment B l B l+1 The distance between them, and key point A i With key point B m Line segment B formed by two adjacent key points m B m and line segment B m B m+1 The distance between them. Key point A i With line segment B l B l The formula for calculating the distance between them is as follows:

[0079]

[0080] Where the denominator is the vector B l B l , the modulus of the vector A i B l , and the modulus of the cross product of the vector A i B l .

[0081] (5) Determine the distance d i , d ai_bl_bl , d ai_bl_bl+1 , d ai_bm_bm , d ai_bm_bm+1 between the key point A min and the four lines mentioned in (4) whether it is less than the expected value d min .

[0082] If there is a distance less than or equal to the expected value d min , it is considered that the two surgical end effectors collide.

[0083] If the distance is greater than the expected value d g , it is considered that the two surgical end effectors do not collide.

[0084] Kinematics model application. The kinematics model provides the necessary motion constraint information for the path planning module, ensuring that the surgical end effector can follow its physical characteristics and motion capabilities when planning the path. When planning the path, the path planning module will generate a safe and efficient motion path according to the motion range and speed limit calculated by the kinematics model.

[0085] Dynamics model. The dynamics model further describes the relationship between the motion of the surgical end effector and the force, including the following aspects: force and motion relationship, dynamics equation, dynamics model application.

[0086] Force and motion relationship. The dynamics model considers the gravity, friction and operating force applied by the operator that the surgical end effector receives during the motion process, which will affect the motion state of the surgical end effector. The following is the specific analysis and formula derivation:

[0087] Gravity F g is the force received by the surgical end effector in the vertical direction, and the influence of gravity on the motion of the surgical end effector needs to be considered when planning the path and avoiding collision, and its size is:

[0088] F 2 = mg

[0089] Where, m is the mass of the surgical end effector; g is the acceleration of gravity, usually 9.8 m / s

[0090] In the movement of the surgical end-effector, the friction force F f will affect the speed and acceleration of the instrument, especially in the start and stop phases of the surgical end-effector. The friction force F f is the resistance generated between the surgical end-effector and the contact surface during movement, which is related to the properties of the contact surface and the normal pressure, and is expressed as:

[0091] F f = μN

[0092] where μ is the friction coefficient; N is the normal pressure, i.e. the force perpendicular to the contact surface.

[0093] The operating force F op is the three-dimensional force applied by the surgeon through the operation end mechanical arm to complete the surgical operation, the size and direction of which depend on the surgical requirements and the design of the master mechanical arm. In the dynamics model, the operating force F op is expressed as:

[0094]

[0095] where β, are the angles, angular velocities, and angular accelerations of the master mechanical arm; x end , v end , a end are the positions, velocities, and accelerations of the end point of the master mechanical arm, i.e. the surgeon control point, which are calculated through the joint angles β, angular velocities and angular accelerations by forward kinematics; K u , B u , M u are the stiffness, damping, and inertia matrices of the master mechanical arm.

[0096] The operating force F op data is sent to the central processing module, affecting the movement state of the surgical end-effector.

[0097] Dynamics equation. The dynamics model describes the movement of the surgical end-effector through the Newton-Euler equation, which combines the Newton equation describing the translational motion of a rigid body and the Euler equation describing the rotational motion of a rigid body. These equations accurately describe the acceleration and velocity changes of the surgical end-effector when subjected to external forces. The dynamics equation is as follows:

[0098]

[0099] where τ is the joint torque matrix; τ f is the friction force F fθ is the joint angle matrix; θ is the joint angular velocity matrix; is the joint angular acceleration matrix; M(θ) is the inertia matrix; is the joint angular acceleration matrix; M(θ) is the inertia matrix; is the centripetal and Coriolis force matrix; G(θ) is the gravity force matrix related to the force F g is the gravity matrix related to the force F op is the force matrix related to the force F op is the force matrix related to the force F

[0100] The dynamics model is applied. The dynamics model provides the necessary information for the dynamic adjustment module, so that it can avoid collision by adjusting the speed and direction of the instrument when a potential collision is detected. For example, when the collision avoidance control module detects that the instrument is close to collision, the motion state of the surgical end effector can be adjusted according to the force and acceleration calculated by the dynamics model.

[0101] The data acquisition module includes position and attitude sensors, clamping force sensors, and endoscope vision acquisition systems. After the sensor data is preprocessed, it is transmitted to the central processing module.

[0102] The position and attitude sensor can monitor the motion state of the surgical end effector in real time, including its position coordinates (X, Y, Z) and attitude angles (pitch angle, yaw angle, roll angle) in three-dimensional space. According to the motor position sensor, the motor angle θ motor is obtained, and the position coordinates (X, Y, Z) and attitude angles (pitch angle, yaw angle, roll angle) of the surgical end effector are obtained through the motor angle θ motor , transmission ratio η and the kinematics of the surgical robot.

[0103] The clamping force sensor is installed on the clamping part of the surgical end effector to monitor the size of the clamping force in real time. The force sensor uses a micro strain gauge array technology and is directly embedded in the clamping joint of the surgical end effector to accurately measure the force applied by the surgical end effector when clamping the surgical tool, avoiding damage to the surgical end effector due to excessive clamping force, and also providing tactile feedback for surgical operation. The measurement accuracy of the force sensor reaches the millinewton level, and the range can be customized to expand to meet the needs of fine surgical operation.

[0104] The endoscope vision acquisition system is used to acquire high-definition visual information of the surgical area in real time, providing intuitive visual feedback for surgical operations. The system includes a high-definition endoscope, an image acquisition unit, and an image processing unit. The high-definition endoscope uses high-resolution, bendable optical fibers or electronic endoscopes, equipped with high-brightness LED light sources, supporting multiple imaging modes, and can adapt to the narrow space of single-hole surgical channels and provide clear images of the surgical area. The image acquisition unit is equipped with high-sensitivity, high-dynamic-range CMOS or CCD image sensors, supporting high-frame-rate acquisition, ensuring the smoothness and real-time nature of surgical operations. The image processing unit uses advanced image processing algorithms to perform real-time enhancement processing on the collected images, including contrast enhancement, sharpening, noise reduction, and color correction, while supporting three-dimensional image reconstruction and intelligent assistance functions such as automatic labeling of lesion areas and tracking of surgical end effector. The image resolution of the endoscope vision acquisition system can reach 4K or higher, and the total delay from image acquisition to display is less than 100 milliseconds, ensuring the accuracy and safety of surgical operations. The system supports the connection of multiple endoscopes and surgical end effectors, has good compatibility and expandability, and meets the needs of complex surgical scenarios.

[0105] The central processing module communicates with the system modeling module, data acquisition module, path planning module, dynamic adjustment module, and coordination mechanism module, and sends and receives control instructions in real time. The central processing module is the control core of the system, responsible for receiving sensor data, monitoring the position and attitude of the instrument in real time, and coordinating the work of each module. The central processing module uses a high-performance computing platform, including a multi-core central processing unit (CPU), a graphics processing unit (GPU), and a field programmable gate array (FPGA). The multi-core CPU is used to handle complex logical operations and data management tasks; the GPU is used to accelerate image processing and parallel computing tasks; the FPGA is used to implement low-latency real-time signal processing. This computing platform supports multi-threading and distributed computing, significantly improving data processing speed and system response capability, and providing real-time feedback control instructions.

[0106] The path planning module includes a planning initial path module and a path smoothing module.

[0107] The initial path planning module constructs a configuration space (C-space) based on the system modeling module and the geometric constraints of the surgical space, as well as surgical requirements and instrument configurations, and plans the optimal path for each instrument. This module considers the current position and orientation of other surgical end effectors to ensure effective collision avoidance during path planning. The RRT algorithm is used to randomly sample and progressively explore the configuration space, starting from the initial configuration point of the surgical end effector, randomly selecting target configuration points, and progressively building a path tree. After each random sampling generates a new configuration point, the path tree is expanded by connecting that point to the nearest node in the path tree. Collision detection is performed on the newly generated path segments; if a collision is detected, the path segment is discarded to ensure path safety. The final path consists of N paths and N+1 turning points P. i The starting point is P0, and the ending point is P. N point.

[0108] like Figure 2 , Figure 3 The path generated by the initial path planning module contains inflections and uneven sections, requiring optimization by the path smoothing module. The path smoothing module improves path smoothness by removing redundant nodes and proposing a novel polynomial curve fitting method. This reduces abrupt changes and wear on the surgical end effector during movement, extends instrument lifespan, and improves the smoothness and precision of surgical procedures. The i-th inflection point P... i The corresponding smooth path process is as follows:

[0109] (1) Determine the basic parameters. Determine the inflection point P. i coordinates (x) pi ,y pi ,z pi ), and two adjacent points P i and P i+1 coordinates (x) pi ,y pi ,z pi ) and (x pi+1 ,y pi+1 ,z pi+1 And based on the coordinates, solve for line segment P. i P i With line segment P i P i+1 The angle β formed i The solution formula is as follows:

[0110]

[0111] (2) Solve for the smoothness λ of the smooth curve. i The solution formula is as follows:

[0112]

[0113] K g The smoothness λ is a constant parameter. i With the included angle β i It increases with the increase of β. i When λ = 0°, i =0; when β i =180°,

[0114] (3) Solve for the starting point of the smooth path End point of the smooth path The solution formula is as follows:

[0115]

[0116] Using the above formula and P i P i P i+1 The coordinates of these three points can be solved. and The coordinates of these two points are represented as follows: and

[0117] (4) Define the function S corresponding to the smooth path. i (t). A smooth path needs to satisfy the following constraints: the starting point is... Point and endpoint are Point, starting direction and P i P i Same direction, termination direction is the same as P i P i+1 The directions are the same. This invention uses the function S corresponding to the smooth path. i (t) is defined as follows:

[0118]

[0119] The dynamic adjustment module is responsible for adjusting and optimizing the planned path in real time during surgery to adapt to the dynamic changes of the surgical end effector and the uncertainties of the surgical environment. The dynamic adjustment module includes a path optimization module, a collision optimization module, a smoothing optimization module, and an interaction optimization module.

[0120] The data processing module receives real-time data from the data acquisition module processed by the central processing module, including the position, attitude, and other information of the surgical end effector, and dynamically updates the path planning in combination with the kinematic model of the surgical end effector and the geometric constraints of the surgical space. An incremental RRT algorithm is used to make local adjustments and extensions based on the original path tree, significantly reducing computation time and resource consumption and improving the real-time performance of the system. The final generated path is verified for safety and feasibility to ensure that each point on the path meets the kinematic constraints of the surgical end effector and does not collide with any obstacles, providing an efficient, flexible, and safe path planning solution for single-port laparoscopic robotic systems and significantly improving the safety and accuracy of surgical operations.

[0121] The collision optimization module realizes real-time collision detection function through real-time force-displacement curve monitoring. When potential collision risk is detected, the collision avoidance strategy is started to adjust the motion direction or speed of the surgical end effector, ensuring surgical safety.

[0122] The smoothing optimization module optimizes and smooths the adjusted path. Through optimization of the evaluation function and adaptive smoothing algorithm, the smoothness and continuity of the path are ensured, the mutation and wear of the surgical end effector during motion are reduced, and the service life of the instrument is prolonged.

[0123] The interaction optimization module supports user interaction and manual intervention functions, allowing surgeons to adjust the path planning in real time when necessary, and providing real-time feedback of path adjustment results and collision detection information through the user interface, helping surgeons make more accurate decisions and significantly improving the safety and accuracy of surgical operations.

[0124] The coordination mechanism module coordinates the actions of multiple surgical end effectors through the central processing module to ensure time and space synchronization between surgical end effectors. The coordination mechanism module uses a fuzzy logic controller to dynamically adjust the actions of surgical end effectors based on their real-time states and surgical requirements, thereby improving the response speed and adaptability of the system.

[0125] The fuzzy logic controller dynamically adjusts the actions of surgical end effectors based on their real-time states (such as position, attitude, speed, etc.) and surgical requirements (such as operation sequence, target position, etc.). The controller processes the input fuzzy information through fuzzy logic reasoning to generate precise control instructions, thereby achieving efficient coordination of multiple surgical end effectors.

[0126] Through the efficient data processing and real-time feedback capability of the central processing module, the coordination mechanism module can accurately coordinate the actions of multiple surgical end-effectors, ensuring the time-space synchronization between the surgical end-effectors. Specifically, the time-space synchronization module dynamically adjusts the motion trajectory and operation sequence of each surgical end-effector according to the kinematic model of the surgical end-effector and real-time sensor data, avoiding collisions between the surgical end-effectors and ensuring the continuity and precision of the surgical operation. Through the time-space synchronization module, the safety and efficiency of the surgery are improved, providing reliable technical support for complex surgical scenarios.

[0127] Obviously, those skilled in the art can make various modifications and variations to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application also intends to include these modifications and variations.

[0128] The contents not described in detail in the specification of the present application are the known technology of those skilled in the art.

Claims

1. A surgical robot cooperative planning and collision avoidance system, characterized in that, include: The surgical end effector module includes several surgical end effectors and endoscopic actuators. Each surgical end effector is equipped with position and clamping force sensors to monitor the movement status and clamping force of the instruments in real time. The system modeling module provides kinematic and dynamic models, as well as corresponding physical constraints, for the data acquisition and path planning modules. The data acquisition module is used to collect the status data of each sensor of the surgical end effector and the visual information of the surgical area where the surgical end effector module is located in real time. The central processing module, based on the kinematic and dynamic models, as well as the received state data and visual information, and combined with the preset kinematic solution method and image processing method, generates surgical environment information and position and orientation information of the surgical end effector. It then sends the surgical environment information to the path planning module and the coordination mechanism module, and the position and orientation information to the dynamic adjustment module and the coordination mechanism module. The path planning module, based on the received surgical environment information, combined with the kinematic and dynamic models and the corresponding physical constraints, plans the optimal path for each surgical end effector, and sends it to the dynamic adjustment module after smoothing. The dynamic adjustment module dynamically adjusts the speed and direction of the surgical end effector based on the received smooth optimal path and position and posture information to avoid collisions, and sends the adjustment results to the coordination mechanism module. The collaborative mechanism module, based on the received surgical environment information, position and posture information, and adjustment results, and in conjunction with surgical requirements, completes the collaborative operation between the surgical end effector and the endoscope actuator under external command control, ensuring the temporal and spatial synchronization between the surgical end effector and the endoscope actuator, thereby achieving the surgical task.

2. The surgical robot cooperative planning and collision avoidance system according to claim 1, characterized in that, The surgical end effector and endoscopic actuator have multiple rotational degrees of freedom and at least one overall translational degree of freedom, enabling spatial operation, and different surgical instruments are installed at the end of the surgical end effector according to surgical needs.

3. The surgical robot cooperative planning and collision avoidance system according to claim 1, characterized in that, The physical constraints include preventing the two surgical end effectors from colliding; the method for determining whether the two surgical end effectors collide includes: (1) Extract the coordinates of all key points on the first surgical end effector and the coordinates of all key points on the second surgical end effector; wherein, the i-th key point A on the first surgical end effector i The coordinates are (x ai ,y ai ,z ai ), the i-th key point B on the second surgical end effector i The coordinates are (x bi ,y bi ,z bi ); (2) Calculate key point A i Distances to all key points on the second surgical end effector; Key point A i With the j-th key point B on the second surgical end effector j distance d ai_bj for: (3) All calculated distances d ai_bj Sort the data, find the two smallest distances, and the distance to key point A. i Two recent key points B l B m ; (4) Solve for key point A i With key point B l Line segment B formed by two adjacent key points l B l and line segment B l B l+1 The distance between them, and key point A i With key point B m Line segment B formed by two adjacent key points m B m and line segment B m B m+1 The distance between them; key point A i With line segment B l B l The distance between them is: The denominator is vector B. l B l The modulus, with the numerator being vector A. i B l With vector A i B l The modulus of the cross product; (5) Based on key point A i The distance d between the four line segments mentioned above ai_bl_bl d ai_bl_bl+1 d ai_bm_bm d ai_bm_bm+1 Determine whether the two surgical end effectors collide.

4. The surgical robot cooperative planning and collision avoidance system according to claim 3, characterized in that, Determine key point A i The distance d between the four line segments mentioned above ai_bl_bl d ai_bl_bl+1 d ai_bm_bm d ai_bm_bm+1 Is it less than the expected value d? min ; If there exists a distance less than or equal to the expected value d min If so, it is considered that the two surgical end effectors collided; If all distances are greater than the expected value d min If so, it is assumed that the two surgical end effectors did not collide.

5. The surgical robot cooperative planning and collision avoidance system according to claim 1, characterized in that, The dynamic model includes: Where τ is the joint moment matrix; τ f It is related to frictional force F f =μN related friction torque matrix; μ is the friction coefficient; N is the normal force, i.e., the force perpendicular to the contact surface; θ is the joint angle matrix; It is the joint angular velocity matrix; M(θ) is the joint angular acceleration matrix; M(θ) is the inertia matrix. It is the centripetal force and Coriolis force matrix; G(θ) is related to gravity F g =mg-related gravity matrix; m is the mass of the surgical end effector; g is the gravitational acceleration; P op It is related to the operating force F op The relevant force matrix.

6. The surgical robot cooperative planning and collision avoidance system according to claim 5, characterized in that, The operating force F op for: Among them, β, These are the angle, angular velocity, and angular acceleration of the robotic arm at the control end; x end v end a end These are the position, velocity, and acceleration of the robotic arm's end point, determined by the joint angle β and angular velocity. angular acceleration K was obtained through forward kinematics calculations. u B u M u These are the stiffness, damping, and inertia matrix of the robotic arm at the control end.

7. The surgical robot cooperative planning and collision avoidance system according to claim 1, characterized in that, The smoothing process includes: (1) Determine the inflection point P i coordinates (x) pi ,y pi ,z pi ), and two adjacent points P i and P i+1 coordinates (x) pi ,y pi ,z pi ) and (x pi+1 ,y pi+1 ,z pi+1 And calculate line segment P based on the coordinates. i P i With line segment P i P i+1 The angle β formed i : (2) Calculate the smoothness λ of the smooth curve. i ; (3) Calculate the starting point of the smooth path End point of the smooth path Using the above formula and P i P i P i+1 Find the coordinates and solve for them. and The coordinates are represented as follows: and (4) Combine the constraints of the smooth path to obtain the function corresponding to the smooth path.

8. The surgical robot cooperative planning and collision avoidance system according to claim 7, characterized in that, The constraints of the smooth path include: the starting point is... Point and endpoint are Point, starting direction and P i P i Same direction, termination direction is the same as P i P i+1 They are in the same direction.

9. A surgical robot cooperative planning and collision avoidance system according to claim 7, characterized in that, The smoothness of the smooth curve is K g This is a constant parameter.

10. A surgical robot cooperative planning and collision avoidance system according to claim 7, characterized in that, The function S corresponding to the smooth path i (t) is

Images