Joint Error Optimization Control Method, Controller and Minimally Invasive Robot

Abstract

An embodiment of the present application provides a joint error optimization control method, a controller, and a minimally invasive robot. The method includes: during the movement of the robotic arm, the controller can obtain the real-time position and real-time speed of the basic joint on the robotic arm in real time. In addition, the controller can also obtain the relative angle information of the basic joint on the robotic arm. The controller can analyze based on the real-time position, real-time speed, and relative angle information of the basic joint to determine the movement trajectory of the redundant joint on the robotic arm during the follow-up process during the movement of the basic joint. Furthermore, based on this movement trajectory, the operation instruction of the redundant joint can be determined, and the redundant joint can be controlled to move according to the movement joint to achieve the follow-up of the redundant joint. This method is used to improve the optimization effect of the joint movement trajectory.

Description

Technical Field

[0001] The present application relates to the field of robot control, and in particular, to a joint error optimization control method, a controller, and a minimally invasive robot. Background Art

[0002] With the development of technology, the application of minimally invasive surgical robots in the medical field has been continuously deepened. In the robotic arm of a minimally invasive surgical robot, the motion errors of redundant joints are likely to interfere with the motions of other joints and the operator's hand.

[0003] Currently, a robotic arm joint motion trajectory optimization method based on the gradient projection algorithm is widely adopted. This method optimizes the motion speed of the robotic arm by constructing a mathematical model, determines the trajectory optimization vector in combination with the task requirements, and further realizes the optimization of the obstacle avoidance path.

[0004] However, the above process involves a large number of gradient calculations and projection operations, and there is a problem of high computational complexity. Therefore, how to improve the joint motion trajectory optimization needs to be further explored. Summary of the Invention

[0005] Embodiments of the present application provide a joint error optimization control method, a controller, and a minimally invasive robot, so as to improve the joint motion trajectory optimization effect.

[0006] In a first aspect, an embodiment of the present application provides a joint error optimization control method, which is applied to the robotic arm of a minimally invasive robot. The robotic arm includes a plurality of basic joints and a redundant joint, and the method includes:

[0007] When the robotic arm moves, obtain the real-time position and real-time speed of the basic joints, and the relative angle information of the basic joints; the relative angle information represents the relative angles between the basic joints and other joints on the robotic arm except the basic joints, including basic joints and redundant joints.

[0008] Generate and execute an operation instruction for the redundant joint according to the real-time position, the real-time speed, and the relative angle information of the basic joints; wherein, the operation instruction is used to control the redundant joint to move following the basic joints.

[0009] Optionally, generating and executing an operation instruction for the redundant joint according to the real-time position, the real-time speed, and the relative angle information of the basic joints includes:

[0010] Calculate the position error and speed error of the basic joints according to the real-time position and the real-time speed of the basic joints.

[0011] Generate the operation instruction of the redundant joint according to the position error, the speed error, and the relative angle information of the basic joint.

[0012] Optionally, calculate the position error and the speed error of the basic joint according to the real-time position and the real-time speed of the basic joint, including:

[0013] Determine the position error of the basic joint according to the difference between the preset target position of the basic joint and the real-time position of the basic joint;

[0014] Determine the speed error of the basic joint according to the difference between the preset target speed of the basic joint and the real-time speed of the basic joint.

[0015] Optionally, the basic joint includes a first basic joint and a second basic joint, where the first basic joint is a basic joint having the same degree of freedom as the redundant joint, and the second basic joint is a basic joint having a collision risk with the redundant joint;

[0016] Generate the operation instruction of the redundant joint according to the position error, the speed error, and the relative angle information of the basic joint, including:

[0017] Determine the adjustment angle of the redundant joint according to the position error and the speed error of the first basic joint, and the relative angle information of the second basic joint;

[0018] Generate the operation instruction of the redundant joint according to the adjustment angle of the redundant joint.

[0019] Optionally, determine the adjustment angle of the redundant joint according to the position error and the speed error of the first basic joint, and the relative angle information of the second basic joint, including:

[0020] Generate the moving trajectory calculation formula according to the position error and the speed error of the first basic joint, and the preset parameters of the moving trajectory calculation formula;

[0021] Optimize the moving trajectory calculation formula according to the relative angle information of the second basic joint, and calculate the adjustment angle of the redundant joint according to the moving trajectory calculation formula.

[0022] Optionally, the method further includes:

[0023] Obtain the initial position of the redundant joint before executing the operation instruction and the final position after executing the operation instruction;

[0024] If the initial position matches the final position, a first warning message is generated, and the first warning message indicates that the response to the operation instruction is abnormal.

[0025] Optionally, the method further includes:

[0026] Based on the real-time position of the redundant joint and the operation instruction, as well as the limit position and singular position of the redundant joint, determine whether the operation instruction controls the redundant joint to avoid the limit position and the singular position;

[0027] Based on the real-time position of the redundant joint and the adjustment angle indicated by the operation instruction, as well as the position information of the second basic joint that has a collision risk with the redundant joint, determine whether the operation instruction controls the redundant joint to avoid the second basic joint;

[0028] If the operation instruction does not control the redundant joint to avoid the limit position or the singular position, or the operation instruction does not control the redundant joint to avoid the second basic joint, a second warning message is generated, and the second warning message indicates that the operation instruction is abnormal.

[0029] Optionally, it further includes:

[0030] If the operation instruction does not control the redundant joint to avoid the limit position or the singular position, or the operation instruction does not control the redundant joint to avoid the second basic joint, record the operation instruction;

[0031] Optimize the preset parameters of the moving trajectory calculation formula according to the recorded operation instruction.

[0032] In a second aspect, an embodiment of the present application provides a joint error optimization control device, which is applied to the robotic arm of a minimally invasive robot. The robotic arm includes multiple joints, and includes:

[0033] An acquisition module, configured to acquire the real-time position and real-time speed of the basic joint, and the relative angle information of the basic joint when the robotic arm moves;

[0034] A processing module, configured to generate and execute an operation instruction for the redundant joint according to the real-time position, the real-time speed, and the relative angle information of the basic joint; wherein, the operation instruction is used to control the redundant joint to follow the basic joint in motion.

[0035] In a third aspect, an embodiment of the present application provides a controller, including: a memory, a processor;

[0036] The memory stores computer execution instructions;

[0037] The processor executes the computer-executable instructions stored in the memory, such that the processor implements the above first aspect and / or various possible implementation manners of the first aspect.

[0038] In a fourth aspect, an embodiment of the present application provides a minimally invasive robot. The robotic arm of the minimally invasive robot includes redundant joints and basic joints; and a controller as described in the third aspect and / or various possible controllers of the third aspect is provided in the minimally invasive robot for implementing the first aspect and / or various possible implementation manners of the first aspect.

[0039] In a fifth aspect, an embodiment of the present application provides a computer-readable storage medium. Computer-executable instructions are stored in the computer-readable storage medium, and when the computer-executable instructions are executed by a processor, they are used to implement the above first aspect and / or various possible implementation manners of the first aspect.

[0040] In a sixth aspect, an embodiment of the present application provides a computer program product, including a computer program, which when executed by a processor implements the above first aspect and / or various possible implementation manners of the first aspect.

[0041] The joint error optimization control method, controller, and minimally invasive robot provided by the embodiments of the present application, by, during the movement of the robotic arm, acquiring in real time the real-time position and real-time speed of the basic joints on the robotic arm; and acquiring the relative angle information of the basic joints on the robotic arm; analyzing based on the real-time position, real-time speed, and relative angle information of the basic joints to determine the movement trajectory of the redundant joints on the robotic arm during the follow-up process when the basic joints are moving; and based on the movement trajectory, determining the operation instructions of the redundant joints and controlling the redundant joints to move according to the movement joints to achieve the follow-up of the redundant joints, improve the optimization of the joint movement trajectory, and improve the obstacle avoidance effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The drawings herein are incorporated into the specification and constitute a part of the specification, showing embodiments consistent with the present application, and are used together with the specification to explain the principles of the present application.

[0043] Figure 1 It is a schematic flowchart of the joint error optimization control method provided by the present application;

[0044] Figure 2 It is a schematic structural diagram of the joint error optimization control device provided by the present application;

[0045] Figure 3 It is a schematic structural diagram of the controller provided by the present application.

[0046] Through the above-mentioned accompanying drawings, specific embodiments of the present application have been shown, and there will be a more detailed description hereinafter. These drawings and textual descriptions are not intended to limit the scope of the concept of the present application in any way, but to illustrate the concept of the present application to those skilled in the art by referring to specific embodiments. Detailed Description of the Specific Embodiments

[0047] Here, the exemplary embodiments will be described in detail, and the examples are shown in the accompanying drawings. When the following description refers to the accompanying drawings, unless otherwise indicated, the same numerals in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present application. On the contrary, they are merely examples of devices and methods consistent with some aspects of the present application as detailed in the appended claims.

[0048] With the development of technology, the application of minimally invasive surgical robots in the medical field has been continuously deepened. Minimally invasive surgical robots generally can include two parts: a doctor control platform and a patient surgical platform. Among them, the master hand is the doctor control platform. The master hand usually includes two 7-degree-of-freedom robotic arms. The doctor can operate these two robotic arms to control the robotic arm of the patient surgical platform to perform the surgery.

[0049] Currently, each revolute joint in the robotic arm can provide at most one degree of freedom. In robotics, a spatial point can have at most 6 degrees of freedom. These 6 degrees of freedom can include three displacements and three rotations. The robotic arm of the doctor control platform can include 7 joints. Among them, the first joint, the second joint, and the third joint exactly provide all the displacement degrees of freedom for the end position, while the fifth joint, the sixth joint, and the seventh joint provide the required rotational degrees of freedom for the end position. Therefore, in the robotic arm of the doctor control platform, the fourth joint that fails to provide degrees of freedom for the end is called a redundant joint.

[0050] The introduction of redundant joints has important significance in minimally invasive surgical robots. It not only improves the flexibility and motion range of the robotic arm, but also enhances the adaptability to complex surgical environments. In addition, redundant joints can improve attitude control, provide more motion options for the robotic arm, thus avoiding potential collision risks and improving the safety and precision of operations.

[0051] In order to make full use of the advantages of redundant joints, the design of the obstacle avoidance algorithm becomes particularly important. The obstacle avoidance algorithm can analyze the workspace of the robotic arm in real time and dynamically adjust the joint angles of the redundant joints according to the changes in the current environment to ensure that the robotic arm can effectively avoid obstacles during movement. At the same time, by optimizing the use of redundant joints, the obstacle avoidance algorithm can not only improve flexibility, but also ensure stability and accuracy during the surgical process. Therefore, the combination of redundant joints and the obstacle avoidance algorithm provides strong technical support for the successful implementation of minimally invasive surgery.

[0052] At present, the motion trajectory optimization method for redundant joints of a robotic arm is generally realized by establishing a mathematical model for robotic arm motion optimization based on a preset gradient projection algorithm. This method can determine the trajectory optimization vector according to the preset task requirements, and then optimize the motion speed of the robotic arm through the mathematical model. This method can determine the motion law of the robotic arm joints between any two adjacent nodes in the initial obstacle avoidance path according to the mathematical model, and optimize the initial obstacle avoidance path according to the motion law and the trajectory optimization vector to obtain the optimized path. That is, this method avoids singular positions and obstacle conflicts through the gradient projection method, but this method has problems such as high computational complexity, the obtained result being a local optimal solution, being sensitive to the shape of obstacles, and being difficult to update in real time.

[0053] Specifically, the gradient projection method will face a large computational burden in real-time applications. Especially in a complex environment, a large number of gradient calculations and projection operations need to be performed each time, which may lead to system response delays and affect the real-time performance of the surgery. Moreover, the gradient projection method is prone to falling into a local optimal solution rather than a global optimal solution. In some cases, the system may not be able to find the best path, thus affecting the flexibility and efficiency of the robotic arm and increasing the surgery time. Also, the gradient projection method is relatively sensitive to the shape and configuration of obstacles. A complex or dynamically changing environment may lead to poor obstacle avoidance effects. This sensitivity reduces the reliability of the system in a changing surgical environment. And although the gradient projection method can avoid singular positions, when dealing with extreme singularities, the gradient projection method may lack sufficient flexibility, resulting in the robotic arm being unable to pass through some key areas smoothly. In addition, in a dynamic surgical environment, the position and state of obstacles may change at any time, while the real-time update ability of the gradient projection method is limited and may not be able to quickly adapt to these changes.

[0054] In summary, there are still many problems with existing solutions such as the gradient projection method when dealing with singular positions and obstacle conflicts in a complex environment. The occurrence of these problems is closely related to the nature of the algorithm itself, the dependence on the environmental model, and the requirements for real-time performance. Therefore, it is still necessary to explore more efficient and flexible obstacle avoidance algorithms to improve the overall performance and safety of minimally invasive surgical robots.

[0055] To this end, the present application provides a method for optimizing joint error control. This method is a redundant joint obstacle avoidance algorithm based on PID control designed based on Twincat3 developed by Beckhoff. During the execution of this method, the controller first needs to determine the joint movement range, limit positions, and singular positions of the robotic arm. Based on the joint movement range, limit positions, and singular positions, the error of this redundant joint during movement can be calculated previously, and then, through the error control method that has been designed in the PID controller, the joint angles of the redundant joint can be controlled and adjusted in real time. The controller can also monitor the obstacle avoidance effect of the redundant joint and optimize the parameters of the error control method in the PID controller according to the obstacle avoidance effect within a certain period of time. Among them, the real-time control and adjustment of the redundant joint and the obstacle avoidance detection of the redundant joint are parallel. During the real-time control process of the redundant joint, within each control cycle, the controller needs to detect obstacles and calculate errors to ensure that the control system can dynamically adapt to environmental changes.

[0056] The present application makes full use of redundant degrees of freedom. By introducing redundant joints, more degrees of freedom of movement are provided, enabling the robotic arm to have greater flexibility and adaptability in complex environments and being able to avoid obstacles more effectively. Moreover, the present application uses the relative angle between two joints for linkage, enhancing the selectivity of the robotic arm in path planning, helping to overcome the problem of local optimal solutions, and improving the overall movement efficiency. In addition, by adjusting the robotic arm posture in real time, the present application can achieve responses to dynamic environmental changes, thereby enhancing the response ability and safety in high-demand scenarios such as surgery. Also, by flexibly adjusting the posture of the redundant joint, the present application can effectively avoid entering singular positions and improve the stability of the robotic arm when performing complex tasks. Although the present application still needs to perform path planning calculations, the use of redundant joints may simplify the calculation process in some cases, reduce the dependence on complex algorithms, and improve real-time performance. The existence of the above improvement points makes the method of the present application have better performance and reliability in the application of minimally invasive surgical robots, promoting the development of related technologies.

[0057] The technical solution of the present application and how the technical solution of the present application solves the above technical problems will be described in detail below with specific embodiments. These several specific embodiments below can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments. The embodiments of the present application will be described below with reference to the accompanying drawings.

[0058] Figure 1Schematic flowchart of the joint error optimization control method provided by this application. The joint error optimization control method of this application is applied to the doctor control platform of a minimally invasive robot. The doctor control platform may include two robotic arms. Each robotic arm may include multiple joints. Optionally, the number of joints on the robotic arm may be 7. Optionally, among the 7 joints, 6 joints may be basic joints for realizing the control of six degrees of freedom of the robot and 1 redundant joint. The execution entity of this application is a controller, such as Figure 1 As shown, the method includes:

[0059] S101. When the robotic arm moves, obtain the real-time position and real-time speed of the basic joints, and the relative angle information of the basic joints. The relative angle information represents the relative angles between the basic joints and the basic joints and redundant joints other than the basic joints on the robotic arm.

[0060] In this embodiment, the operator can control the movement of the robotic arm by operating the robotic arm. During the movement of the robotic arm, the controller can obtain the real-time position and real-time speed of the basic joints on the robotic arm in real time. In addition, the controller can also obtain the relative angle information of the basic joints on the robotic arm. Optionally, the relative angle information of the basic joints may include the relative angles between the basic joints and the basic joints and redundant joints other than the basic joints on the robotic arm. For example, in the 7-joint robotic arm, the relative angle information of each basic joint may include the relative angles between the basic joint and the other 5 basic joints and 1 redundant joint. That is, the relative angle information of the basic joint may include 6 relative angles.

[0061] S102. Generate and execute an operation instruction for the redundant joint according to the real-time position, real-time speed, and relative angle information of the basic joint. The operation instruction is used to control the redundant joint to follow the basic joint in motion.

[0062] In this embodiment, the controller can analyze according to the real-time position, real-time speed, and relative angle information of the basic joint to determine the movement trajectory of the redundant joint on the robotic arm during the movement of the basic joint in the follow-up process. Furthermore, based on the movement trajectory, the operation instruction of the redundant joint can be determined, and the redundant joint can be controlled to move according to the moving joint to realize the follow-up of the redundant joint, thereby further improving the obstacle avoidance effect and operation flexibility of the mechanism.

[0063] In one example, the specific process of the controller generating the operation instruction for the redundant joint may include:

[0064] Step 1. Calculate the position error and speed error of the basic joint according to the real-time position and real-time speed of the basic joint.

[0065] In this step, the controller can first calculate the position error of the base joint based on the real-time position of the base joint, and calculate the speed error of the base joint based on the real-time speed of the base joint. Specifically, a preset target position of the base joint can be preset in the controller. Optionally, the target position can be calculated based on the hand position of the operator. The controller can determine the position error of the base joint according to the difference between the preset target position of the base joint and the real-time position of the base joint. A preset target speed of the base joint can also be preset in the controller. The controller can determine the speed error of the base joint according to the difference between the preset target speed of the base joint and the real-time speed of the base joint.

[0066] Step 2: Generate and execute the operation instruction of the redundant joint according to the position error, speed error, and relative angle information of the base joint.

[0067] In this step, the controller will calculate the final position of the redundant joint according to the position error and speed error of the base joint. When the redundant joint moves to the final position, it can assist each joint of the robotic arm to get closer to the target position. Also, the controller can plan the movement trajectory of the redundant joint according to the relative angle information of the base joint, so that the redundant joint can better avoid the base joint during the movement and prevent collisions. Furthermore, based on the movement trajectory, the controller can generate the operation instruction of the redundant joint, so that the redundant joint moves to the final position according to the movement trajectory.

[0068] Specifically, the base joint includes a first base joint and a second base joint. Among them, the first base joint is a base joint with the same degree of freedom as the redundant joint. The second base joint is a base joint with a collision risk with the redundant joint. For example, the first base joint can be the 6th joint, the second base joint can be the 5th joint, and the redundant joint can be the 4th joint. The controller can calculate the final position that the 4th joint needs to reach according to the position error and speed error of the 6th joint and the relative angle information of the 5th joint. The final position can be achieved by adjusting the joint angle of the 4th joint. The joint angle that the 4th joint needs to adjust is the adjustment angle. When the 4th joint adjusts the pose of the robotic arm according to the adjustment angle, the robot can better match the operation of the operator and prevent the robotic arm from colliding with obstacles. The obstacles can include the robotic arm itself and the main body of the surgical robot. The controller can generate the operation instruction of the redundant joint according to the adjustment angle of the redundant joint.

[0069] Specifically, the controller can calculate the adjustment angle according to the movement trajectory calculation formula. The controller can substitute the two error values of the position error and the speed error of the first basic joint into the movement trajectory calculation formula, and combine the preset parameters of the movement trajectory calculation formula to generate the movement trajectory calculation formula. The movement trajectory calculation formula can be:

[0070]

[0071] where u(t) is the output of the controller. e(t) is the position error. t is the time coefficient. K p is the proportional gain. K i is the integral gain. K d is the derivative gain. is the error accumulation information obtained by integrating the position error over time. is the rate of change of the position error over time.

[0072] Based on the above movement trajectory calculation formula, according to the relative angle information of the second basic joint, the calculation result of the movement trajectory calculation formula can be reduced, and the calculation result of the movement trajectory calculation formula can be quickly calculated. The calculation result indicates the adjustment angle of the redundant joint. The calculation process based on the movement trajectory calculation formula can be regarded as a PID control process.

[0073] In the joint error optimization control method provided by the embodiments of the present application, during the movement of the robotic arm, the controller can real-time obtain the real-time position and real-time speed of the basic joints on the robotic arm. And, the controller can also obtain the relative angle information of the basic joints on the robotic arm. The controller can analyze according to the real-time position, real-time speed and relative angle information of the basic joints to determine the movement trajectory of the redundant joints on the robotic arm during the follow-up process during the movement of the basic joints. Furthermore, based on the movement trajectory, the operation instruction of the redundant joints can be determined, and the redundant joints can be controlled to move according to the movement joints to realize the follow-up of the redundant joints. Through the control of the redundant joints in the present application, the obstacle avoidance effect and operation flexibility of the machine are further improved. And, by introducing relative angle information in the calculation process in the present application, the calculation time is shortened, and the reaction time is shortened to within 50 milliseconds, ensuring that the robot can quickly make adjustments in a dynamic environment, which has been greatly improved compared with the prior art. And, through the use of PID control in the present application, the joint movement is smoother, reducing jitter and vibration, and improving the overall operation stability. The improvement of this stability can further reduce the collision and failure rate, and reduce the maintenance cost.

[0074] Based on the above embodiments, the controller can also generate the following warning information:

[0075] S201. Obtain the position of the redundant joint after executing the operation instruction and the final position indicated by the operation instruction.

[0076] In this step, the controller can also obtain the position of the redundant joint after executing the operation instruction of the redundant joint. The controller can also obtain the final position that the redundant joint needs to reach indicated in the operation instruction.

[0077] S202. If the position does not match the final position, generate a first warning message, where the first warning message characterizes that the response of the operation instruction is abnormal.

[0078] In this step, the controller can determine whether the position matches the final position. If the position matches the final position, it indicates that the redundant joint has executed the operation instruction normally. If the position does not match the final position, it indicates that the redundant joint has not completed the movement. At this time, the controller can generate a first warning message. The first warning message is used to remind the user that the response of the operation instruction of the redundant joint is abnormal. The abnormal response of the redundant joint may increase the risk of collision and attention should be paid to avoidance.

[0079] Furthermore, the controller can also make the following judgments:

[0080] S203. Determine whether the operation instruction controls the redundant joint to avoid the limit position and the singular position according to the real-time position of the redundant joint and the operation instruction, as well as the limit position and the singular position of the redundant joint.

[0081] In this step, the limit position of the redundant joint can be pre-stored in the controller. If the redundant joint will move to the limit position or even exceed the limit position according to the operation instruction, it indicates that the operation instruction is abnormal. Also, the controller can determine whether the adjustment angle obtained during the calculation of the movement trajectory calculation formula is a singular position. Optionally, the singular position can be an operation instruction obtained when there are multiple solutions to the movement trajectory calculation formula. If the operation instruction indicates that the redundant joint moves to the singular position, it indicates that the operation instruction is abnormal.

[0082] S204. Determine whether the operation instruction controls the redundant joint to avoid the second basic joint according to the real-time position of the redundant joint, the adjustment angle indicated by the operation instruction, and the position information of the second basic joint that has a collision risk with the redundant joint.

[0083] In this step, the controller can obtain the position of the obstacle. The obstacle may include a second base joint that may collide with the redundant joint. Or the obstacle may also be the main body part of the surgical robot, etc. The controller can determine whether the redundant joint may collide with the obstacle after moving according to the running instruction based on the real-time position of the redundant joint and the running instruction. If a collision may occur, the running instruction is abnormal.

[0084] S205. If the running instruction does not control the redundant joint to avoid the limit position or the singular position, or the running instruction does not control the redundant joint to avoid the second base joint, a second warning message is generated, and the second warning message indicates that the running instruction is abnormal.

[0085] In this step, if the controller determines that the redundant joint is abnormal according to the above steps 203 and 204, the controller can generate a second warning message. The second warning message is used to remind the user that there is an abnormality in the running instruction. That is, further, there is an abnormality in the generation process of the running instruction. And the second warning instruction is also used to remind the user that the redundant joint may collide with an obstacle.

[0086] Optionally, the controller can generate an obstacle avoidance effect report according to the execution results of the above steps S201 to S205, and record the result of whether obstacle avoidance is successful.

[0087] Based on the above embodiments, the controller can also optimize the preset parameters:

[0088] S301. If the running instruction does not control the redundant joint to avoid the limit position or the singular position, or the running instruction does not control the redundant joint to avoid the second base joint, record the running instruction.

[0089] In this step, the controller can also record the running instruction when generating the warning message in the above steps S201 to S205. Optionally, the controller can also record the real-time position and real-time speed of the base joint that generates the running instruction, as well as information such as the position of the redundant joint before and after movement.

[0090] S302. Optimize the preset parameters of the movement trajectory calculation formula according to the recorded running instruction.

[0091] In this step, the controller can periodically analyze and process the recorded information. Optionally, the period can be a preset duration. For example, the preset duration can be 1 day, 1 week, 1 month, etc. The controller can optimize the preset parameters of the moving trajectory calculation formula through this analysis. Specifically, the controller can analyze the effect of the PID control parameters based on the change information. Based on the analysis result, the controller can adjust , , the three parameters, thereby improving the system stability and response speed.

[0092] Figure 2 FIG. Figure 2 is a schematic structural diagram of the joint error optimization control device provided by the present application, which is applied to the robotic arm of a minimally invasive robot. The robotic arm includes multiple joints, such as

[0093] An acquisition module 401, configured to acquire the real-time position and real-time speed of the base joint, and the relative angle information of the base joint when the robotic arm moves; the relative angle information represents the relative angle between the base joint and other joints.

[0094] A processing module 402, configured to generate and execute an operation instruction for the redundant joint according to the real-time position, the real-time speed, and the relative angle information of the base joint; wherein, the operation instruction is used to control the redundant joint to follow the base joint in motion.

[0095] Optionally, the processing module 402 is configured to:

[0096] Calculate the position error and speed error of the base joint according to the real-time position and the real-time speed of the base joint;

[0097] Generate and execute an operation instruction for the redundant joint according to the position error, the speed error, and the relative angle information of the base joint.

[0098] Optionally, the processing module 402 is configured to:

[0099] Determine the position error of the base joint according to the difference between the preset target position of the base joint and the real-time position of the base joint;

[0100] Determine the speed error of the base joint according to the difference between the preset target speed of the base joint and the real-time speed of the base joint.

[0101] Optionally, the base joint includes a first base joint and a second base joint, where the first base joint is a base joint having the same degrees of freedom as the redundant joint, and the second base joint is a base joint having a risk of collision with the redundant joint; a processing module 402, configured to:

[0102] Determine an adjustment angle of the redundant joint according to the position error, the velocity error of the first base joint, and the relative angle information of the second base joint;

[0103] Generate an operation instruction for the redundant joint according to the adjustment angle of the redundant joint.

[0104] Optionally, the processing module 402 is configured to:

[0105] Generate the moving trajectory calculation formula according to the position error and the velocity error of the first base joint, and preset parameters of the moving trajectory calculation formula;

[0106] Optimize the moving trajectory calculation formula according to the relative angle information of the second base joint, and calculate the adjustment angle of the redundant joint according to the moving trajectory calculation formula.

[0107] Optionally, the processing module 402 is configured to:

[0108] Obtain the position where the redundant joint is located after executing the operation instruction and the final position indicated by the operation instruction;

[0109] If the location does not match the final position, generate a first warning message, where the first warning message indicates that the operation instruction response is abnormal.

[0110] Optionally, the processing module 402 is configured to:

[0111] Determine whether the operation instruction controls the redundant joint to avoid the limit position and the singular position according to the real-time position and the operation instruction of the redundant joint, and the limit position and the singular position of the redundant joint;

[0112] Determine whether the operation instruction controls the redundant joint to avoid the second base joint according to the real-time position of the redundant joint, the adjustment angle indicated by the operation instruction, and the position information of the second base joint having a risk of collision with the redundant joint;

[0113] If the operation instruction does not control the redundant joint to avoid the limit position or the singular position, or the operation instruction does not control the redundant joint to avoid the second base joint, generate a second warning message, where the second warning message indicates that the operation instruction is abnormal.

[0114] Optionally, the processing module 402 is configured to:

[0115] If the running instruction does not control the redundant joint to avoid the limit position or the singular position, or the running instruction does not control the redundant joint to avoid the second base joint, record the running instruction;

[0116] Optimize the preset parameters of the moving trajectory calculation formula according to the recorded running instruction.

[0117] The joint error optimization control device provided in this embodiment can execute the method provided in the above method embodiment, and its implementation principle and technical effect are similar, which will not be elaborated here in this embodiment.

[0118] Figure 3 It is a schematic structural diagram of the controller provided in this application. As Figure 3 shown, the controller 500 provided in this embodiment includes: at least one processor 501 and a memory 502. Optionally, the device 50 further includes a communication component 503. Among them, the processor 501, the memory 502, and the communication component 503 are connected through a bus 504.

[0119] In a specific implementation process, at least one processor 501 executes the computer execution instructions stored in the memory 502, so that at least one processor 501 executes the above method.

[0120] The specific implementation process of the processor 501 can refer to the above method embodiment, and its implementation principle and technical effect are similar, which will not be elaborated here in this embodiment.

[0121] In the above embodiment, it should be understood that the processor may be a central processing unit (Central Processing Unit, CPU), or other general-purpose processors, digital signal processors (Digital Signal Processor, DSP), application specific integrated circuits (Application Specific Integrated Circuit, ASIC), etc. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc. The steps of the method disclosed in combination with the invention can be directly implemented by the hardware processor, or implemented by a combination of hardware and software modules in the processor.

[0122] The memory may include a high-speed memory (Random Access Memory, RAM), and may also include a non-volatile memory (Non-volatile Memory, NVM), such as at least one disk memory.

[0123] The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, an Extended Industry Standard Architecture (EISA) bus, or the like. The bus can be divided into an address bus, a data bus, a control bus, etc. For the sake of convenience in representation, the buses in the drawings of the present application are not limited to only one bus or one type of bus.

[0124] The present application also provides a computer program product, including a computer program which, when executed by a processor, implements the above method.

[0125] The present application also provides a computer-readable storage medium storing computer-executable instructions, which, when executed by a processor, implement the above method.

[0126] The above-mentioned readable storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as a static random access memory, an electrically erasable programmable read-only memory, an erasable programmable read-only memory, a programmable read-only memory, a read-only memory, a magnetic memory, a flash memory, a magnetic disk or an optical disk. The readable storage medium can be any available medium accessible by a general-purpose or special-purpose computer.

[0127] An exemplary readable storage medium is coupled to the processor, enabling the processor to read information from and write information to the readable storage medium. Of course, the readable storage medium can also be a component of the processor. The processor and the readable storage medium can be located in an Application Specific Integrated Circuit (ASIC). Of course, the processor and the readable storage medium can also exist as discrete components in a device.

[0128] The division of units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed coupling or direct coupling or communication connection between each other can be an indirect coupling or communication connection through some interfaces, devices or units, and can be in electrical, mechanical or other forms.

[0129] The unit described as a separate component may or may not be physically separated, and the component shown as a unit may or may not be a physical unit, that is, it may be located in one place or distributed across multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0130] In addition, in each embodiment of the present invention, each functional unit can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit.

[0131] If the function is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on such understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art or a part of this technical solution can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods in each embodiment of the present invention. The aforementioned storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), magnetic disks, or optical discs that can store program codes.

[0132] Those of ordinary skill in the art can understand that all or part of the steps of implementing the above method embodiments can be completed by hardware related to program instructions. The aforementioned program can be stored in a computer-readable storage medium. When this program is executed, it executes the steps including the above method embodiments; and the aforementioned storage medium includes: various media such as ROM, RAM, magnetic disks, or optical discs that can store program codes.

[0133] Finally, it should be noted that: After considering the specification and practicing the invention disclosed herein, those skilled in the art will readily think of other implementation schemes of the present invention. The present invention aims to cover any variations, uses, or adaptable changes of the present invention. These variations, uses, or adaptable changes follow the general principles of the present invention and include common general knowledge or conventional technical means in the technical field not disclosed in the present invention. It is not limited to the exact structures described above and shown in the drawings, and various modifications and changes can be made without departing from its scope. The scope of the present invention is only limited by the appended claims.

Claims

1. A method for optimizing joint error control, characterized in that, A robotic arm applied to a minimally invasive robot, the robotic arm including a plurality of basic joints and a redundant joint, the method comprising: When the robotic arm moves, obtaining the real-time position and real-time speed of the basic joints, and the relative angle information of the basic joints; the relative angle information characterizes the relative angles between the basic joints and the basic joints and redundant joints other than the basic joints on the robotic arm; Generating and executing an operation instruction for the redundant joint according to the real-time position, the real-time speed and the relative angle information of the basic joints; wherein, the operation instruction is used to control the redundant joint to move following the basic joints; Wherein, generating and executing an operation instruction for the redundant joint according to the real-time position, the real-time speed and the relative angle information of the basic joints includes: Calculating the position error and speed error of the basic joints according to the real-time position and the real-time speed of the basic joints; Generating and executing an operation instruction for the redundant joint according to the position error, the speed error and the relative angle information of the basic joints; Wherein, the basic joints include a first basic joint and a second basic joint, wherein, the first basic joint is a basic joint having the same degree of freedom as the redundant joint, and the second basic joint is a basic joint having a risk of collision with the redundant joint; Wherein, generating an operation instruction for the redundant joint according to the position error, the speed error and the relative angle information of the basic joints includes: Determining the adjustment angle of the redundant joint according to the position error and the speed error of the first basic joint, and the relative angle information of the second basic joint; Generating an operation instruction for the redundant joint according to the adjustment angle of the redundant joint.

2. The method according to claim 1, characterized in that Calculating the position error and speed error of the basic joints according to the real-time position and the real-time speed of the basic joints includes: Determining the position error of the basic joint according to the difference between the preset target position of the basic joint and the real-time position of the basic joint; Determining the speed error of the basic joint according to the difference between the preset target speed of the basic joint and the real-time speed of the basic joint.

3. The method according to claim 1, wherein Determining the adjustment angle of the redundant joint according to the position error and the speed error of the first basic joint, and the relative angle information of the second basic joint includes: Generating a moving trajectory calculation formula according to the position error and the speed error of the first basic joint, and the preset parameters of the moving trajectory calculation formula; Optimizing the moving trajectory calculation formula according to the relative angle information of the second basic joint, and calculating the adjustment angle of the redundant joint according to the moving trajectory calculation formula.

4. The method according to any one of claims 1 to 3, characterized in that, The method further includes: Obtaining the position where the redundant joint is located after executing the operation instruction and the final position indicated by the operation instruction; If the location does not match the final position, a first warning message is generated, which indicates that the response to the operation instruction is abnormal.

5. The method according to any one of claims 1 to 3, characterized in that, The method further includes: Determining whether the operation instruction controls the redundant joint to avoid the limit position and the singular position according to the real-time position of the redundant joint and the operation instruction, as well as the limit position and the singular position of the redundant joint; Determining whether the operation instruction controls the redundant joint to avoid the second basic joint according to the real-time position of the redundant joint and the adjustment angle indicated by the operation instruction, as well as the position information of the second basic joint at risk of collision with the redundant joint; If the operation instruction does not control the redundant joint to avoid the limit position or the singular position, or the operation instruction does not control the redundant joint to avoid the second basic joint, a second warning message is generated, which indicates that the operation instruction is abnormal.

6. The method according to claim 5, characterized in that, The method further includes: If the operation instruction does not control the redundant joint to avoid the limit position or the singular position, or the operation instruction does not control the redundant joint to avoid the second basic joint, record the operation instruction; Optimize the preset parameters of the moving trajectory calculation formula according to the recorded operation instruction.

7. An articular error optimization control device, characterized in that, Applied to the robotic arm of a minimally invasive robot, the robotic arm includes a plurality of basic joints and a redundant joint. The device includes: An acquisition module, configured to acquire the real-time position and real-time speed of the basic joint, and the relative angle information of the basic joint when the robotic arm moves; A processing module, configured to generate and execute an operation instruction for the redundant joint according to the real-time position, the real-time speed, and the relative angle information of the basic joint; wherein, the operation instruction is used to control the redundant joint to move following the basic joint; Among them, the processing module is specifically configured to: Calculate the position error and speed error of the basic joint according to the real-time position and the real-time speed of the basic joint; Generate and execute an operation instruction for the redundant joint according to the position error, the speed error, and the relative angle information of the basic joint; Among them, the basic joint includes a first basic joint and a second basic joint, where the first basic joint is a basic joint having the same degree of freedom as the redundant joint, and the second basic joint is a basic joint at risk of collision with the redundant joint; Among them, in the processing module, generating an operation instruction for the redundant joint according to the position error, the speed error, and the relative angle information of the basic joint specifically includes: Determining the adjustment angle of the redundant joint according to the position error and the speed error of the first basic joint, and the relative angle information of the second basic joint; Generating an operation instruction for the redundant joint according to the adjustment angle of the redundant joint.

8. A controller, characterized in that, Including: A memory, a processor; The memory stores computer execution instructions; The processor executes the computer-executable instructions stored in the memory, such that the processor performs the method according to any one of claims 1-6.

9. A minimally invasive robot, characterized in that, The robotic arm of the minimally invasive robot includes redundant joints and a base joint; a controller as shown in claim 8 is provided in the minimally invasive robot for optimizing the error of the redundant joints.

10. A computer-readable storage medium, characterized in that, Computer-executable instructions are stored in the computer-readable storage medium, and when the computer-executable instructions are executed by a processor, they are used to implement the method according to any one of claims 1-6.

11. A computer program product, characterized in that, It includes a computer program which, when executed by a processor, implements the method according to any one of claims 1-6.

Images