Multi-degree-of-freedom obstacle avoidance control system and control method for cardiac surgery robot

Abstract

The invention provides a multi-degree-of-freedom obstacle avoidance control system and control method for a cardiac surgical robot, the system comprises a main control unit, a parameter detection unit, a data storage unit, a communication unit and a motion control algorithm unit, meanwhile, an out-of-environment obstacle avoidance method is designed for the surgical robot, and an obstacle avoidance method is designed for a scene in a surgery. An obstacle avoidance method in the environment is designed, the flexibility of the slave hand is improved, the motion real-time performance and cooperative performance of a single arm and multiple arms are enhanced, the accuracy and safety of the whole operation are improved, the control precision is improved, collision of the multiple arms in a narrow space is avoided, the obstacle avoidance function is achieved, and harm to a patient is reduced.

Description

Technical Field

[0001] The present invention belongs to the field of surgical robots, and in particular relates to a multi-degree-of-freedom obstacle avoidance control system and a control method for a cardiac surgical robot. Background Art

[0002] Surgical robots can reduce the physical labor of doctors during surgery, while achieving the purpose of precision surgery, so that patients have minimal trauma, less blood loss, less postoperative infection, and faster postoperative recovery. Cardiac surgery robots are currently more demanding in terms of technology. Existing cardiac surgery robots are divided into master hands and slave hands. The slave hands are usually equipped with multiple robotic arms, and the working spaces of each robotic arm overlap. In actual surgical scenes, there may also be various tools, obstacles, etc.

[0003] In the existing technology, the invention patent CN115830320 A applied by Huake Precision (Beijing) Medical Technology Co., Ltd. is named as a medical robot and its automatic obstacle avoidance method. It uses image acquisition equipment to obtain two-dimensional image data and three-dimensional point cloud data of the surgical scene, and determines the safe movement path of the robotic arm according to the three-dimensional point cloud of the obstacle. Although this patent realizes the obstacle avoidance function to a certain extent, there are still many shortcomings: the flexibility and freedom of the slave hand are not high, and most of them are driven by the master hand. As a device that enters the human body and operates, the obstacle avoidance ability and function of the slave hand mainly depend on the response of the master hand and the doctor; the obstacle avoidance function is added, but it is not accurate enough, and each of its actions is related to the patient's life safety. It does not solve the problem of how to avoid collisions between the various robotic arms and the collision between the robotic arms and the external environment; when the master hand fails, the robotic arm cannot autonomously plan a collision-free executable path; the processing capacity of the slave hand is insufficient, the processing speed is slow, and the synchronization is poor. Summary of the invention

[0004] The purpose of the present invention is to solve the problems existing in the prior art and to provide a multi-degree-of-freedom obstacle avoidance control system and control method for a cardiac surgical robot. The multi-degree-of-freedom obstacle avoidance control system of the present invention is a slave-hand control system, which improves the flexibility of the slave-hand, improves the accuracy and safety of the entire operation, improves the control accuracy, enhances the obstacle avoidance capability, and reduces harm to the patient. The control method of the slave-hand control system for a cardiac surgical robot adds a slave-hand multi-arm obstacle avoidance control method, adds a slave-hand multi-degree-of-freedom collaborative control method, adds force perception, visual perception and other detection systems, and adds algorithm redundancy design and comparative analysis.

[0005] The present invention provides a multi-degree-of-freedom obstacle avoidance control system for a cardiac surgery robot. The control system comprises: a main control unit, a parameter detection unit, a data storage unit, a communication unit, and a motion control algorithm unit.

[0006] Furthermore, the main control unit receives commands from the master hand control system through the communication unit, selects the control algorithm in the motion control algorithm unit according to the data from the parameter detection unit, drives and controls the multi-degree-of-freedom multi-arm robot, controls the multi-axis degrees of freedom, and performs real-time multi-arm obstacle avoidance processing, and stores and calls the data. This main control unit designs a method for the main control unit to control multiple arms, improves the flexibility of the slave hand, enhances the real-time performance and coordination performance of the single arm and multiple arms, improves the accuracy and safety of the entire operation, improves the control accuracy, avoids the collision of multiple arms in a small space, and has an obstacle avoidance function, reducing harm to the patient.

[0007] Furthermore, the parameter detection unit includes a temperature information detection unit, a torque detection unit, a position detection unit, and a visual information detection unit, which respectively obtain the temperature, torque, position, and image information of the environment, and provide the slave main control unit with control algorithm data and obstacle avoidance parameter support, thereby ensuring the accurate control performance of the slave control system, improving the reliability of the system, ensuring the parameter accuracy of each degree of freedom of the robot system and each arm, and improving the coordination, intelligence, and reliability of each degree of freedom of the system;

[0008] Furthermore, the motion control algorithm unit, which includes obstacle avoidance algorithms for each robotic arm of the slave hand and control algorithms for each degree of freedom of the arm, analyzes and interprets the motion states and surrounding environment of the multiple arms of the slave hand system robot to perform obstacle avoidance processing, and simultaneously analyzes signals such as torque for multi-axis control processing. By adopting the above-mentioned surgical robot obstacle avoidance method, the surgical robot can complete its own collision detection and external collision detection in a complex surgical environment, avoid obstacles in the preset moving path of the surgical robot, and successfully complete the designated surgical work, thereby improving the practicality and intelligence of the surgical robot.

[0009] Furthermore, the communication unit includes CAN communication, Ethernet communication, etc. These communications are mainly used for communication between the master hand control system and the slave hand control system, receiving instructions from the master hand control system, and performing multiple communication modes each time data is transmitted to compare data instructions to execute operations, avoiding surgical problems caused by communication and improving the safety, reliability, real-time and stability of the system.

[0010] Furthermore, the data storage unit is used to access and save the data and reference data processed by the main control unit, and at the same time store the design algorithm and various model data, which is conducive to data calling, ensures data traceability, facilitates data comparison, and stores control and paths.

[0011] The present invention also provides a multi-degree-of-freedom obstacle avoidance control method for a cardiac surgical robot, which improves the flexibility of the hands, enhances the real-time movement performance and coordination performance of single-arm and multi-arm movements, improves the accuracy and safety of the entire operation, improves control accuracy, avoids collisions of multiple arms in a small space, and has an obstacle avoidance function, reducing harm to patients.

[0012] The method mainly includes obstacle avoidance strategies outside the environment and obstacle avoidance strategies inside the environment. The outside environment refers to the outside of the human body, and the inside environment refers to the inside of the human body. Obstacle avoidance outside the environment mainly involves multiple arms between sleeves (protective sleeves outside the multiple arms), which are obstacles to each other, and the sleeve is used as an obstacle. The sleeve and multiple arms are obstacles to each other. The sleeve is a fixed obstacle and the multiple arms are active obstacles. At this time, obstacle avoidance is strictly speaking possible, but the collision force needs to be controlled within a range to ensure relative smoothness; obstacle avoidance inside the environment mainly involves obstacle avoidance in the narrow space of the heart of multiple arms and the human body, that is, each arm uses objects such as the heart body and blood vessels as obstacles. At this time, the obstacles are not allowed to collide or the collision force has almost no effect on human body safety. The multi-degree-of-freedom obstacle avoidance control method process first determines the environment through a camera. If it is outside the environment, an obstacle avoidance strategy outside the environment is implemented; if it is inside the environment, an obstacle avoidance strategy inside the environment is implemented.

[0013] The obstacle avoidance strategy process outside the environment is as follows:

[0014] (1) According to the image acquisition, two states are determined: all arms are in the sleeve and the body of all arms is in the sleeve and the end of the arm is outside the sleeve. When the arm is in the sleeve, the end of the arm and the sleeve, and the arms are mutually obstructed. When the end of the arm is outside the sleeve, the ends of the arm are mutually obstructed.

[0015] (2) When the arm is in the sleeve, each arm in the multiple arms in the sleeve is regarded as a cylinder, and the sleeve is regarded as a hollow cylinder, where the distance between each two arm is defined as: is the distance between the two arms, d a , d b is the radius of the arm (this radius can be the same or different, depending on the actual situation of the arm). The distance between the arm and the sleeve is defined as: is the distance between the axis of a certain arm and the inner wall of the sleeve, d i is the radius of the arm. Collect the force feedback data of the torque sensor, where τ1 is the force feedback between the arms, and τ2 is the force feedback between the arm and the sleeve;

[0016] (3) Kinematic modeling of the entire motion control system:

[0017] X K+1 =XK +AU K (1)

[0018] Among them, X K is the state at the previous moment, X K+1 is the state at the future moment, which represents the state information between each two arms and the state information between the arm and the sleeve. A is the time coefficient, U K is the control quantity input state;

[0019] (4) Designing a corresponding loss function; In order to make trajectory planning more accurate and reliable, the present invention designs two models of loss functions, performs loss analysis and comparison, and uses the optimal solution as the optimal path.

[0020]

[0021] Among them, x mpc (k), x mpc-cbf (k) is the expected state at time k, x(k+1) is the expected state at time k+1, and n is the equal time period;

[0022] (5) Calculate the accuracy probability of the expected value and the value of the true distribution, and confirm that the more accurate formula in formula (2) or (3) is used at this moment. The smaller the accuracy probability, the more accurate it is. The method for calculating the accuracy probability is as follows:

[0023]

[0024] Where H(p,q) is the accuracy probability, p(y j ) is the true probability, q(y j ) is the predicted probability;

[0025] (6) Set constraints. According to comparative analysis, the biggest risks for cardiac surgery robots are collision force and distance. Therefore, distance and collision force are used as constraints, and two optimization equations are established according to step (4). The constraint condition of the torque between the arms is:

[0026]

[0027] in, is the maximum force of the collision, and the collision force can be set according to the actual situation;

[0028] The constraint condition of the torque between the sleeves is:

[0029]

[0030] in, is the maximum force of the collision, and the collision force can be set according to the actual situation; α(k) is the confidence coefficient. The distance constraint between the arms is:

[0031]

[0032] in is the shortest distance between the arms, which can be set according to the actual working conditions. The distance constraint between the arm and the sleeve is:

[0033]

[0034] in It is the shortest distance between the booms and can be set according to actual working conditions.

[0035] (7) Use the application formula confirmed in step (5) to generate a point with a small probability at each moment as a collision-free point to generate a path. Because when performing heart surgery, the larger the collision-free distance is, the better. Instead, the distance should be controlled at the edge of the collision. The smaller the better. When multiple robotic arms move in the sleeve, the sleeve and the robotic arms are regarded as obstacles. The exit of the sleeve is used as a plane reference. It can safely extend the exit without damaging the obstacle as a subclass of reaching the target point. Calculate a path point M to reach the target point at each time step. t , as a collection of waypoint subclasses

[0036] (8) The optimal collision-free point set W1 between the arms and between the arms and the sleeve can be calculated through step (5) and formulas (5) and (6).

[0037] (9) Through step (5) and formulas (7) and (8), the optimal collision-free point set W2 between the arms and between the arms and the sleeve can be calculated.

[0038] (10) Determine whether there is interference: Compare W1 and W2 with W. When the difference between each moment of the path point set and the collision-free point under the constraint condition is less than the threshold, it means there is no interference and it is the optimal path. If there is interference, reset the constraint condition until the optimal interference-free path is calculated.

[0039] When it is outside the sleeve, the difference from being inside the sleeve is that the constraints between the arm and the sleeve are removed, the other steps remain unchanged, and the optimal path is calculated.

[0040] The obstacle avoidance strategy in the described environment is as follows:

[0041] (1) Obtaining various three-dimensional configuration data of human hearts and storing them in a storage unit for use in data comparison and correction;

[0042] (2) Obtaining raw sensor data through the camera carried by the robotic arm, and collecting force data and position data through the position sensor and torque sensor;

[0043] (3) Select one of the stored three-dimensional heart data, and use the three-dimensional heart configuration data as an initial target point of the robot arm; form a new three-dimensional model through the image data collected by the image, as a new real-time target point, compare the real-time three-dimensional model target point with the initial three-dimensional model target point, and obtain an initial navigation path according to the instructions. Call the obstacle avoidance method outside the environment, calculate the path point for complete obstacle avoidance, optimize the initial path, and obtain the final path point. The constraints need to be set according to the specific conditions in the environment.

[0044] (4) This path point is marked and matched with the real-time three-dimensional model, and each constraint condition is used as the baseline for obstacle rewards and penalties. When it is greater than the baseline, a penalty is imposed, and when it is less than the baseline, a reward is imposed. Each target point is used as the target point reward and penalty baseline. When the position difference between the motion path point and the target point is less than the position threshold, a reward is imposed, and when it is greater than the threshold, the reward is zero. Through multiple surgical simulations and practices, the system is able to autonomously and accurately avoid obstacles, and the data is stored in the memory for the next call.

[0045] Furthermore, the force control algorithm steps for multi-DOF joints are as follows:

[0046] (1) Collect the parameters of the six-dimensional force at the end and the parameters of the one-dimensional force at the single end through the sensor;

[0047] (2) Inversely solve the six-dimensional force to obtain the parameters of each single-end one-dimensional force;

[0048] (3) Compare the inverse solution parameters with the actual collected one-dimensional force, is the one-dimensional force of the i-th joint and the inverse one-dimensional force of the j-th joint i=j, when When it is greater than the designed force threshold, it means that the force of this joint is too large, and the output of this joint is reset. When it is less than the threshold, continue to follow the given output to achieve real-time coordinated control of each degree of freedom.

[0049] The beneficial effects of the present invention are as follows:

[0050] (1) The present invention designs a multi-degree-of-freedom obstacle avoidance control method for a cardiac surgical robot. The control system improves the flexibility of the hands, the accuracy and safety of the entire operation, improves the control accuracy, facilitates the coordinated control of multiple degrees of freedom, and reduces harm to patients.

[0051] (2) An obstacle avoidance method outside the environment (i.e., obstacle avoidance between the sleeve and multiple arms) is designed for the surgical robot itself. This method avoids destructive collisions when the multiple arms move in and outside the sleeve (i.e., damage to the equipment due to excessive collision force), improves the reliability and stability of the equipment, ensures the integrity of the equipment itself, and facilitates the multiple arms to move smoothly out of the sleeve.

[0052] (3) Aiming at the scenarios during surgery, an obstacle avoidance method within the environment (i.e., obstacle avoidance between the arm and the heart and human body) was designed. This method ensures the patient's personal safety, improves the reliability of the surgery, and reduces the risk of the surgery.

[0053] (4) The present invention designs a multi-degree-of-freedom obstacle avoidance control method for a cardiac surgical robot, adds a redundant software design method, performs data verification and review, and ensures the accurate performance of data points and the reliability of trajectories.

[0054] (5) The present invention designs a multi-degree-of-freedom obstacle avoidance control method for a cardiac surgical robot, which increases the autonomous learning capability. Based on multiple simulations and tests, the robot's own obstacle avoidance capability is enhanced, thereby improving the intelligence of the device.

[0055] (6) The present invention designs a control method for a multi-degree-of-freedom arm, improves the coordination capability of multiple freedoms, ensures the synchronization performance of multiple arms and multiple degrees of freedom, and improves the stability of the equipment.

[0056] (7) This design incorporates storage devices and acquisition devices, which are helpful in providing data support for the main control unit algorithm and storing and facilitating retrieval. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] The accompanying drawings constituting a part of the present application are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention:

[0058] Figure 1 This is a schematic diagram of the composition of a slave control system for a cardiac surgery robot according to an embodiment of the present invention;

[0059] Figure 2 is a schematic diagram of the composition of a parameter detection unit according to an embodiment of the present invention;

[0060] Figure 3 This is a flow chart of a multi-degree-of-freedom obstacle avoidance control method for a cardiac surgery robot according to an embodiment of the present invention;

[0061] Figure 4 It is a flow chart of the force control algorithm of the multi-degree-of-freedom joint according to an embodiment of the present invention. DETAILED DESCRIPTION

[0062] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings 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 should fall within the scope of protection of the present invention.

[0063] This embodiment provides a hand control system for a cardiac surgery robot, such as Figure 1 As shown, the control system includes: an FPGA main control unit, a parameter detection unit, a data storage unit, a communication unit, and a motion control algorithm unit.

[0064] Furthermore, the main control unit receives commands from the master control system through the communication unit, selects the control algorithm in the motion control algorithm unit according to the data from the parameter detection unit, drives and controls the multi-degree-of-freedom multi-arm robot, controls multi-axis degrees of freedom, and performs real-time multi-arm obstacle avoidance processing, and stores and calls the data.

[0065] Furthermore, the parameter detection unit includes a temperature information detection unit, a torque detection unit, a position detection unit, and a visual information detection unit, which respectively obtain the temperature, torque, position, and image information of the environment, and provide the slave main control unit with control algorithm data and obstacle avoidance parameter support, thereby ensuring the accurate control performance of the slave control system, improving the reliability of the system, ensuring the parameter accuracy of each degree of freedom of the robot system and each arm, and improving the coordination, intelligence, and reliability of each degree of freedom of the system;

[0066] Furthermore, the motion control algorithm unit, which includes obstacle avoidance algorithms for each robotic arm of the slave hand and control algorithms for each degree of freedom of the arm, analyzes and interprets the motion states and surrounding environment of the multiple arms of the slave hand system robot to perform obstacle avoidance processing, and simultaneously analyzes signals such as torque for multi-axis control processing. By adopting the above-mentioned surgical robot obstacle avoidance method, the surgical robot can complete its own collision detection and external collision detection in a complex surgical environment, avoid obstacles in the preset moving path of the surgical robot, and successfully complete the designated surgical work, thereby improving the practicality and intelligence of the surgical robot.

[0067] Furthermore, the communication unit includes CAN communication, Ethernet communication, etc. These communications are mainly used for communication between the master hand control system and the slave hand control system, receiving instructions from the master hand control system, and performing multiple communication modes each time data is transmitted to compare data instructions to execute operations, avoiding surgical problems caused by communication and improving the safety, reliability, real-time and stability of the system.

[0068] Furthermore, the data storage unit is used to access and save the data and reference data processed by the main control unit, and at the same time store the design algorithm and various model data, which is conducive to data calling, ensures data traceability, facilitates data comparison, and stores control and paths.

[0069] The embodiment of the present invention also provides a multi-degree-of-freedom obstacle avoidance control method for a cardiac surgical robot, which improves the flexibility of the hands, enhances the real-time movement performance and coordination performance of single-arm and multi-arm movements, improves the accuracy and safety of the entire operation, improves control accuracy, avoids collisions of multiple arms in a small space, and has an obstacle avoidance function, thereby reducing harm to patients.

[0070] The method mainly includes obstacle avoidance strategies outside the environment and obstacle avoidance strategies inside the environment, such as Figure 3 As shown. The environment outside refers to the outside of the human body, and the environment inside refers to the inside of the human body. The obstacle avoidance outside the environment is mainly that multiple arms are between the sleeves (protective sleeves outside the multiple arms), which are obstacles to each other, and the sleeve is used as an obstacle. The sleeve and multiple arms are obstacles to each other. The sleeve is a fixed obstacle and the multiple arms are active obstacles. At this time, the obstacle avoidance can strictly speaking be a collision, but the collision force needs to be controlled within the range to ensure relative smoothness; the obstacle avoidance inside the environment is mainly the obstacle avoidance of multiple arms and the narrow space of the heart inside the human body, that is, each arm uses the heart body, blood vessels and other objects as obstacles. At this time, the obstacles are not allowed to collide or the collision force hardly affects the safety of the human body. The multi-degree-of-freedom obstacle avoidance control method process first determines the environment through the camera. If it is outside the environment, the obstacle avoidance strategy outside the environment is implemented, and if it is inside the environment, the obstacle avoidance strategy inside the environment is implemented.

[0071] The obstacle avoidance strategy outside the environment is as follows:

[0072] (1) According to the image acquisition, two states are determined: all arms are in the sleeve and the body of all arms is in the sleeve and the end of the arm is outside the sleeve. When the arm is in the sleeve, the end of the arm and the sleeve, and the arms are mutually obstructed. When the end of the arm is outside the sleeve, the ends of the arm are mutually obstructed.

[0073] (2) When the arm is in the sleeve, each arm in the multiple arms in the sleeve is regarded as a cylinder, and the sleeve is regarded as a hollow cylinder. Then the distance between each two arm is defined as: is the distance between the two arms, d a , d b is the radius of the arm (this radius can be the same or different, depending on the actual situation of the arm). The distance between the arm and the sleeve is defined as: is the distance between the axis of a certain arm and the inner wall of the sleeve, di is the radius of the arm. Collect the force feedback data of the torque sensor, where τ1 is the force feedback between the arms, and τ2 is the force feedback between the arm and the sleeve;

[0074] (3) Kinematic modeling of the entire motion control system:

[0075] X K+1 =X K +AU K (1)

[0076] Among them, X K is the state at the previous moment, X K+1 is the state at the future moment, which represents the state information between each two arms and the state information between the arm and the sleeve. A is the time coefficient, U K is the control quantity input state;

[0077] (4) Designing a corresponding loss function; In order to make trajectory planning more accurate and reliable, the present invention designs two models of loss functions, performs loss analysis and comparison, and uses the optimal solution as the optimal path.

[0078]

[0079] Among them, x mpc (k), x mpc-cbf (k) is the expected state at time k, x(k+1) is the expected state at time k+1, and n is the equal time period;

[0080] (5) Calculate the accuracy probability of the expected value and the value of the true distribution, and confirm that the more accurate formula in formula (2) or (3) is used at this moment. The smaller the accuracy probability, the more accurate it is. The method for calculating the accuracy probability is as follows:

[0081]

[0082] Where H(p,q) is the accuracy probability, p(y j ) is the true probability, q(y j ) is the predicted probability;

[0083] (6) Set constraints. According to comparative analysis, the biggest risks for cardiac surgery robots are collision force and distance. Therefore, distance and collision force are used as constraints, and two optimization equations are established according to step (4). The constraint condition of the torque between the arms is:

[0084]

[0085] in, is the maximum force of the collision, and the collision force can be set according to the actual situation; the constraint condition of the moment between the arm and the sleeve is:

[0086]

[0087] in, is the maximum force of the collision, and the collision force can be set according to the actual situation; α(k) is the confidence coefficient. The distance constraint between the arms is:

[0088]

[0089] in is the shortest distance between the arms, which can be set according to the actual working conditions. The distance constraint between the arm and the sleeve is:

[0090]

[0091] in It is the shortest distance between the booms and can be set according to actual working conditions.

[0092] (7) Use the application formula confirmed in step (5) to generate a point with a small probability at each moment as a collision-free point to generate a path. Because when performing heart surgery, the larger the collision-free distance is, the better. Instead, the distance should be controlled at the edge of the collision. The smaller the better. When multiple robotic arms move in the sleeve, the sleeve and the robotic arms are regarded as obstacles. The exit of the sleeve is used as a plane reference. It can safely extend the exit without damaging the obstacle as a subclass of reaching the target point. Calculate a path point M to reach the target point at each time step. t , as a collection of waypoint subclasses

[0093] (8) The optimal collision-free point set W1 between the arms and between the arms and the sleeve can be calculated through step (5) and formulas (5) and (6).

[0094] (9) The optimal collision-free point set W2 between the arms and between the arms and the sleeve can be calculated through step (5) and formulas (7) and (8).

[0095] (10) Determine whether there is interference: Compare W1 and W2 with W. When the difference between each moment of the path point set and the collision-free point under the constraint condition is less than the threshold, it means there is no interference and it is the optimal path. If there is interference, reset the constraint condition until the optimal interference-free path is calculated.

[0096] When it is outside the sleeve, the difference from being inside the sleeve is that the constraints between the arm and the sleeve are removed, the other steps remain unchanged, and the optimal path is calculated.

[0097] The obstacle avoidance strategy process in the environment is as follows:

[0098] (1) Obtaining various three-dimensional configuration data of human hearts and storing them in a storage unit for use in data comparison and correction;

[0099] (2) Obtaining raw sensor data through the camera carried by the robotic arm, and collecting force data and position data through the position sensor and torque sensor;

[0100] (3) Select one of the stored three-dimensional heart data, and use the three-dimensional heart configuration data as an initial target point of the robot arm; form a new three-dimensional model through the image data collected by the image, as a new real-time target point, compare the real-time three-dimensional model target point with the initial three-dimensional model target point, and obtain an initial navigation path according to the instructions. Call the obstacle avoidance method outside the environment, calculate the path point for complete obstacle avoidance, optimize the initial path, and obtain the final path point. The constraints need to be set according to the specific conditions in the environment.

[0101] (4) This path point is marked and matched with the real-time three-dimensional model, and each constraint condition is used as the baseline for obstacle rewards and penalties. When it is greater than the baseline, a penalty is imposed, and when it is less than the baseline, a reward is imposed. Each target point is used as the target point reward and penalty baseline. When the position difference between the motion path point and the target point is less than the position threshold, a reward is imposed, and when it is greater than the threshold, the reward is zero. Through multiple surgical simulations and practices, the system is able to autonomously and accurately avoid obstacles, and the data is stored in the memory for the next call.

[0102] For the force control algorithm of multi-degree-of-freedom joints, Figure 4 As shown, the steps are as follows:

[0103] (1) Collect the parameters of the six-dimensional force at the end and the parameters of the one-dimensional force at the single end through the sensor;

[0104] (2) Inversely solve the six-dimensional force to obtain the parameters of each single-end one-dimensional force;

[0105] (3) Compare the inverse solution parameters with the actual collected one-dimensional force, is the one-dimensional force of the i-th joint and the inverse one-dimensional force of the j-th joint i=j, when When it is greater than the designed force threshold, it means that the force of this joint is too large, and the output of this joint is reset. When it is less than the threshold, continue to follow the given output to achieve real-time coordinated control of each degree of freedom.

[0106] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A multi-degree-of-freedom obstacle avoidance control system for a cardiac surgery robot, characterized in that: The control system is a slave control system, including: a main control unit, a parameter detection unit, a data storage unit, a communication unit, and a motion control algorithm unit; wherein, The communication unit receives commands from the main control system, the parameter detection unit detects environmental information, the motion control algorithm unit stores the control algorithm, and the main control unit selects the control algorithm based on the received commands and the environmental information to control the multi-axis degrees of freedom of the multi-arm robot and perform real-time multi-arm obstacle avoidance processing, and stores and calls the data.

2. A multi-degree-of-freedom obstacle avoidance control system for a cardiac surgery robot according to claim 1, characterized in that: The parameter detection unit includes a temperature information detection unit, a torque detection unit, a position detection unit, and a visual information detection unit, which respectively obtain the temperature, torque, position, and image information of the environment.

3. The multi-degree-of-freedom obstacle avoidance control system for a cardiac surgery robot according to claim 1, characterized in that: The data storage unit stores and accesses the data and reference data processed by the main control unit, and stores the design algorithm and various model data.

4. A multi-degree-of-freedom obstacle avoidance control method applied to a control system as claimed in any one of claims 1 to 3, characterized in that: It includes obstacle avoidance methods outside the environment and obstacle avoidance methods inside the environment.

5. The multi-degree-of-freedom obstacle avoidance control method according to claim 4, characterized in that: The obstacle avoidance method outside the environment includes the following steps: According to the image acquisition, two states of the arm are determined: all the arm are in the sleeve, the body of all the arm is in the sleeve, and the end of the arm is outside the sleeve; when the arm is in the sleeve, the end of the arm and the sleeve, and the arms are obstacles to each other, and when the end of the arm is outside the sleeve, the ends of the arm are obstacles to each other.

6. The multi-degree-of-freedom obstacle avoidance control method according to claim 5, characterized in that: When all arms are in the sleeve, the obstacle avoidance method outside the environment is as follows: (1) Each arm in the multi-arm in the sleeve is regarded as a cylinder, and the sleeve is regarded as a hollow cylinder. Then the distance between each two arms is defined as: is the distance between the two arms, d a , d b is the radius of the arm; the distance between the arm and the sleeve is defined as: is the distance between the axis of a certain arm and the inner wall of the sleeve, d i is the radius of the arm; collect force feedback data of the torque sensor, where τ1 is the force feedback between the arms, and τ2 is the force feedback between the arm and the sleeve; (2) Kinematic modeling of the entire motion control system: X K+1 =X K +AU K (1) Among them, X K is the state at the previous moment, X K+1 is the state at the future moment, which represents the state information between each two arms and the state information between the arm and the sleeve. A is the time coefficient, U K is the control quantity input state; (3) Designing a corresponding loss function; in order to make the trajectory planning more accurate and reliable, the present invention designs two models of loss functions, performs loss analysis and comparison, and uses the optimal solution as the current optimal path; Among them, x mpc (k), x mpc-cbf (k) is the expected state at time k, x(k+1) is the expected state at time k+1, and n is the equal time period; (4) Calculate the accuracy probability of the expected value and the value of the true distribution, and confirm that the more accurate formula in formula (2) or (3) is used at this moment. The smaller the accuracy probability, the more accurate it is. The method for calculating the accuracy probability is as follows: Where H(p,q) is the accuracy probability, p(y j ) is the true probability, q(y j ) is the predicted probability; (5) Set the constraint conditions. According to the comparative analysis, take the distance and collision force as the constraint conditions, and establish two optimization equations according to step (4); the constraint condition of the moment between the arms is: in, is the maximum force of the collision, and the magnitude of the collision force can be set according to the actual situation; the constraint condition of the moment between the arm and the sleeve is: in, is the maximum force of the collision, and the collision force can be set according to the actual situation; α n (k), α m (k is the confidence factor; the distance constraint between the arms is: in is the shortest distance between the arms, which can be set according to the actual working conditions. The distance constraint between the arm and the sleeve is: in It is the shortest distance between the booms and can be set according to the actual working conditions; (6) Use the application formula confirmed in step (5) to generate a point with a small probability at each moment as a collision-free point to generate a path. When multiple robotic arms move in the sleeve, the sleeve and the robotic arms are regarded as obstacles, and the sleeve exit is regarded as a plane reference. It can safely extend the exit without damaging the obstacle as a subclass of reaching the target point. Calculate a path point M to reach the target point at each time step. t , as a collection of waypoint subclasses (7) The optimal collision-free point set W1 between the arms and between the arms and the sleeve can be calculated through step (5) and formula (5) (6); (8) The optimal collision-free point set W2 between the arms and between the arms and the sleeve can be calculated through step (5) and formulas (7) and (8); (9) Determine whether there is interference: Compare W1 and W2 with W. When the difference between each moment of the path point set and the non-collision point under the constraint condition is less than the threshold, there is no interference and it is the optimal path. If there is interference, reset the constraint condition until the optimal non-interference path is calculated.

7. The multi-degree-of-freedom obstacle avoidance control method according to claim 6, characterized in that: When the body of all arms is inside the sleeve and the end of the arm is outside the sleeve, the difference between the obstacle avoidance method and the arm inside the sleeve is that the constraints between the arm and the sleeve are removed, and the other processes remain unchanged, and the optimal path is calculated.

8. The multi-degree-of-freedom obstacle avoidance control method according to claim 4, characterized in that: The steps of the obstacle avoidance method in the environment are as follows: (1) Obtaining various three-dimensional configuration data of human hearts and storing them in a storage unit for use in data comparison and correction; (2) Obtaining raw sensor data through the camera carried by the robotic arm, and collecting position data and force data through the position sensor and torque sensor; (3) selecting one of the stored three-dimensional cardiac configuration data, and using the three-dimensional cardiac configuration data as an initial target point of the robot arm; forming a new three-dimensional model through the image data collected by the image, and using the new real-time target point as the real-time three-dimensional model target point, and comparing the real-time three-dimensional model target point with the initial three-dimensional model target point, and obtaining an initial navigation path according to the instruction; calling the obstacle avoidance method outside the environment, and the constraint conditions need to be set according to the specific conditions in the environment, and calculating the path point of complete obstacle avoidance, optimizing the initial path, and obtaining the final path point; (4) This path point is marked and matched with the real-time three-dimensional model, and each constraint condition is used as the baseline for obstacle rewards and penalties. When it is greater than the baseline, a penalty is imposed, and when it is less than the baseline, a reward is imposed. Each target point is used as the target point reward and penalty baseline. When the position difference between the motion path point and the target point is less than the position threshold, a reward is imposed, and when it is greater than the threshold, the reward is zero. Through multiple surgical simulations and practices, the system is able to autonomously and accurately avoid obstacles, and the data is stored in the memory for the next call.

9. The multi-degree-of-freedom obstacle avoidance control method according to claim 8, characterized in that: The force control algorithm for multi-degree-of-freedom joints is as follows: (1) Collect the parameters of the six-dimensional force at the end and the parameters of the one-dimensional force at the single end through the sensor; (2) Inversely solve the six-dimensional force to obtain the parameters of each single-end one-dimensional force; (3) Compare the inverse solution parameters with the actual collected one-dimensional force, is the one-dimensional force of the i-th joint and the inverse one-dimensional force of the j-th joint, i = j, when When it is greater than the designed force threshold, it means that the force of this joint is too large, and the output of this joint is reset. When it is less than the threshold, continue to follow the given output to achieve real-time coordinated control of each degree of freedom.

Images