Joint error optimization control method, controller and minimally invasive robot

Abstract

The embodiment of the invention provides a joint error optimization control method, a controller and a minimally invasive robot. The method comprises the steps that in the moving process of the mechanical arm, a controller can obtain the real-time position and the real-time speed of a basic joint on the mechanical arm in real time. And the controller can also obtain relative angle information of the basic joints on the mechanical arm. The controller can perform analysis according to the real-time position, the real-time speed and the relative angle information of the basic joint to determine the moving track of the redundant joint on the mechanical arm in the follow-up process in the moving process of the basic joint. And furthermore, an operation instruction of the redundant joint can be determined based on the moving track, and the redundant joint is controlled to move according to the moving joint, so that follow-up of the redundant joint is realized. The method is used for improving the joint motion trail optimization effect.

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 science and technology, the application of minimally invasive surgical robots in the medical field is becoming more and more in-depth. In the robotic arms of minimally invasive surgical robots, the motion errors of redundant joints can easily interfere with the motion of other joints and the operator's hands.

[0003] At present, the robot joint motion trajectory optimization method based on the gradient projection algorithm is widely used. This method optimizes the robot movement speed by constructing a mathematical model, and determines the trajectory optimization vector based on the task requirements, thereby optimizing the obstacle avoidance path.

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

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

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

[0007] When the robotic arm moves, the real-time position and real-time speed of the basic joint, as well as the relative angle information of the basic joint are obtained; the relative angle information represents the relative angle between the basic joint and other joints of the robotic arm except the basic joint and the redundant joint;

[0008] According to the real-time position, the real-time speed and the relative angle information of the basic joint, an operation instruction of the redundant joint is generated and executed; wherein the operation instruction is used to control the redundant joint to follow the movement of the basic joint.

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

[0010] Calculating a position error and a velocity error of the basic joint according to the real-time position and the real-time velocity of the basic joint;

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

[0012] Optionally, calculating 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 includes:

[0013] 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;

[0014] The speed error of the basic joint is determined 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, 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 collision risk with the redundant joint;

[0016] Generating an 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] Determining an 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] An operation instruction of the redundant joint is generated according to the adjustment angle of the redundant joint.

[0019] Optionally, 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:

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

[0021] The movement trajectory calculation formula is optimized according to the relative angle information of the second basic joint, and the adjustment angle of the redundant joint is calculated according to the movement trajectory calculation formula.

[0022] Optionally, the method further includes:

[0023] Acquire 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 alarm message is generated, wherein the first alarm message indicates that the operation instruction response is abnormal.

[0025] Optionally, the method further includes:

[0026] According to the real-time position of the redundant joint and the operation instruction, as well as the extreme position and the singular position of the redundant joint, determining whether the operation instruction controls the redundant joint to avoid the extreme position and the singular position;

[0027] 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 that has a collision risk with the redundant joint;

[0028] If the operating instruction fails to control the redundant joint to avoid the extreme position or the singular position, or the operating instruction fails to control the redundant joint to avoid the second basic joint, a second alarm message is generated, and the second alarm message indicates that the operating instruction is abnormal.

[0029] Optionally, it also includes:

[0030] If the operation instruction does not control the redundant joint to avoid the extreme position or the singular position, or the operation instruction does not control the redundant joint to avoid the second basic joint, then the operation instruction is recorded;

[0031] The preset parameters of the movement trajectory calculation formula are optimized according to the recorded operation instructions.

[0032] In a second aspect, an embodiment of the present application provides a joint error optimization control device, which is applied to a mechanical arm of a minimally invasive robot, wherein the mechanical arm includes a plurality of joints, including:

[0033] An acquisition module, used for acquiring 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 is used to generate and execute an operation instruction of 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 movement of the basic joint.

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

[0036] The memory stores computer-executable instructions;

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

[0038] In a fourth aspect, an embodiment of the present application provides a minimally invasive robot, wherein the robotic arm of the minimally invasive robot includes redundant joints and basic joints; the minimally invasive robot is provided with a controller such as the third aspect and / or various possible implementations of the third aspect, for implementing the first aspect and / or various possible implementations of the first aspect.

[0039] In a fifth aspect, an embodiment of the present application provides a computer-readable storage medium, in which computer-executable instructions are stored. When the computer-executable instructions are executed by a processor, they are used to implement the first aspect above and / or various possible implementations 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 methods of the first aspect.

[0041] The joint error optimization control method, controller and minimally invasive robot provided in the embodiments of the present application obtain the real-time position and real-time speed of the basic joints on the robotic arm in real time during the movement of the robotic arm; and obtain the relative angle information of the basic joints on the robotic arm; analyze 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 following process of the basic joint movement; based on the movement trajectory, the operation instructions of the redundant joints can be determined, and the redundant joints can be controlled to move according to the moving joints to achieve the following means of the redundant joints, thereby improving the optimization of the joint motion trajectory and improving the obstacle avoidance effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0043] Figure 1 A schematic diagram of the flow chart of the joint error optimization control method provided in this application;

[0044] Figure 2 A schematic diagram of the structure of the joint error optimization control device provided in this application;

[0045] Figure 3 A schematic diagram of the structure of the controller provided in this application.

[0046] The above drawings have shown clear embodiments of the present application, which will be described in more detail later. These drawings and text descriptions are not intended to limit the scope 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

[0047] Exemplary embodiments will be described in detail herein, examples of which are shown in the accompanying drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The implementations described in the following exemplary embodiments do not represent all implementations consistent with the present application. Instead, 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 science and technology, the application of minimally invasive surgical robots in the medical field continues to deepen. Minimally invasive surgical robots can usually include two parts: a doctor control platform and a patient surgical platform. Among them, the main hand is the doctor control platform. The main hand usually includes two 7-joint robotic arms. Doctors can operate the two robotic arms to control the robotic arms of the patient surgical platform to perform surgery.

[0049] At present, each rotation joint in a robotic arm can provide at most one degree of freedom. In robotics, a spatial point can have at most 6 degrees of freedom. The 6 degrees of freedom can include three displacements and three rotations. The robotic arm of the doctor-controlled platform can include 7 joints. Among them, the first joint, the second joint, and the third joint just provide all the displacement degrees of freedom for the end position, and 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-controlled platform, the fourth joint in the middle that fails to provide degrees of freedom for the end is called a redundant joint.

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

[0051] In order to fully utilize the advantages of redundant joints, the design of obstacle avoidance algorithm becomes particularly important. The obstacle avoidance algorithm can analyze the working space of the robot 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 robot 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 surgery. Therefore, the combination of redundant joints and 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 implemented based on a robotic arm motion optimization mathematical model established 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 joint 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 result obtained is a local optimal solution, it is sensitive to the shape of the obstacle, and it is difficult to update in real time.

[0053] Specifically, the gradient projection method faces a large computational burden in real-time applications. Especially in complex environments, a large number of gradient calculations and projection operations are required each time, which may cause system response delays and affect the real-time performance of the surgery. In addition, 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, which affects the flexibility and efficiency of the robot arm and increases the operation time. Moreover, the gradient projection method is also sensitive to the shape and configuration of obstacles. Complex or dynamically changing environments may result in poor obstacle avoidance. This sensitivity reduces the reliability of the system in a changing surgical environment. In addition, although the gradient projection method can avoid singular positions, it may lack sufficient flexibility when dealing with extreme singularities, resulting in the robot arm being unable to pass through certain key areas smoothly. In addition, in a dynamic surgical environment, the position and state of obstacles may change at any time, and the real-time update capability of the gradient projection method is limited and may not be able to adapt to these changes quickly.

[0054] In summary, existing solutions such as the gradient projection method still have many problems when dealing with singular positions and obstacle conflicts in complex environments. These problems are closely related to the nature of the algorithm itself, its dependence on the environmental model, and the requirements of 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 joint error optimization control method. The method is a redundant joint obstacle avoidance algorithm based on PID control designed based on Twincat3 developed by Beckhoff. During the execution of the method, the controller first needs to determine the joint motion range, extreme position and singular position of the robot arm. Based on the joint motion range, extreme position and singular position, the error of the redundant joint during the movement process can be calculated before, and then the error control method designed in the PID controller is completed to control and adjust the joint angle of the redundant joint 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 joints are parallel to the obstacle avoidance detection of the redundant shutdown. In the process of real-time control of the redundant joints, in 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, and by introducing redundant joints, provides more degrees of freedom of movement, so that the robot arm has greater flexibility and adaptability in complex environments, and can avoid obstacles more effectively. In addition, the present application uses the relative angle between the two joints for linkage, which enhances the selectivity of the robot arm in path planning, helps to overcome the problem of local optimal solution, and improves the overall motion efficiency. In addition, the present application can achieve the response to dynamic environmental changes by adjusting the posture of the robot arm in real time, thereby improving the responsiveness and safety in high-demand scenarios such as surgery. In addition, the present application effectively avoids entering singular positions and improves the stability of the robot arm when performing complex tasks by flexibly adjusting the posture of the redundant joints. 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-mentioned improvements makes the method of the present application have better performance and reliability in the application of minimally invasive surgical robots, and promotes 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-mentioned technical problems are described in detail below with specific embodiments. The following specific embodiments 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 in conjunction with the accompanying drawings.

[0058] Figure 1The flowchart of the joint error optimization control method provided in the present application is applied to the doctor control platform of the 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, the 7 joints may include 6 basic joints and 1 redundant joint that can be used to realize the control of the six degrees of freedom of the robot. The execution subject of the present application is a controller, such as Figure 1 As shown, the method includes:

[0059] S101, when the robot arm moves, obtain the real-time position and real-time speed of the basic joint and the relative angle information of the basic joint. The relative angle information represents the relative angle between the basic joint and the basic joints and redundant joints other than the basic joint on the robot 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. And, 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 angle between the basic joint 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 angle 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 operation instructions of the redundant joints according to the real-time position, real-time speed and relative angle information of the basic joints, wherein the operation instructions are used to control the redundant joints to follow the movement of the basic joints.

[0062] In this embodiment, the controller can analyze 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 robot arm during the following process of the basic joint movement. 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 achieve the following movement of the redundant joint, thereby further improving the obstacle avoidance effect and operational flexibility of the machine.

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

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

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

[0066] Step 2: Generate and execute the operation instructions of the redundant joints according to the position error, speed error, and relative angle information of the basic joints.

[0067] In this step, the controller calculates the final position of the redundant joint based on the position error and speed error of the basic joint. When the redundant joint moves to the final position, it can assist the various joints of the robotic arm to get closer to the target position. In addition, the controller can also plan the movement trajectory of the redundant joint based on the relative angle information of the basic joint, so that the redundant joint can better avoid the basic joint during the movement and avoid collision. Furthermore, based on the movement trajectory, the controller can generate an operation instruction for the redundant joint so that the redundant joint moves to the final position according to the movement trajectory.

[0068] Specifically, the basic joint includes a first basic joint and a second basic joint. Among them, the first basic joint is a basic joint with the same degree of freedom as the redundant joint. The second basic joint is a basic joint that has a risk of collision with the redundant joint. For example, the first basic joint may be the 6th joint, the second basic joint may be the 5th joint, and the redundant joint may be the 4th joint. The controller may calculate the final position that the 4th joint needs to reach based on the position error, speed error, and relative angle information of the 6th joint, and 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 posture of the robot arm according to the adjustment angle, the machine can better match the operator's operation and avoid the robot arm from colliding with an obstacle. The obstacle may include the robot arm itself and the surgical robot body. The controller may generate an operation instruction for 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 position error and the speed error of the first basic joint into the movement trajectory calculation formula, and generate the movement trajectory calculation formula in combination with the preset parameters of 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 differential gain. It is the accumulated error information obtained by integrating the position error over time. is the rate of change of position error with time.

[0072] Based on the above movement trajectory calculation formula, the calculation result of the movement trajectory calculation formula can be reduced according to the relative angle information of the second basic joint, 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] The joint error optimization control method provided by the embodiment of the present application, during the movement of the mechanical arm, the controller can obtain the real-time position and real-time speed of the basic joint on the mechanical arm in real time. And, the controller can also obtain the relative angle information of the basic joint on the mechanical arm. 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 mechanical arm during the movement of the basic joint. Then, 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 achieve the following movement of the redundant joint. The present application further improves the obstacle avoidance effect and operational flexibility of the machine by controlling the redundant joint. In addition, the present application shortens the calculation time and shortens the reaction time to within 50 milliseconds by introducing relative angle information in the calculation process, ensuring that the robot can make adjustments quickly in a dynamic environment, which is greatly improved compared with the prior art. And, the present application makes the joint movement smoother, reduces jitter and vibration, and improves the overall operational stability through the use of PID control. The improvement of this stability can further reduce the maintenance cost by reducing collision and failure rate.

[0074] Based on the above embodiment, 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 may also obtain the position of the redundant joint after executing the operation instruction of the redundant joint. The controller may also obtain the final position that the redundant joint needs to reach as indicated in the operation instruction.

[0077] S202: If the current location does not match the final location, generate a first alarm message, wherein the first alarm message indicates that the operation instruction response is abnormal.

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

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

[0080] S203. Determine, based on the real-time position of the redundant joint and the operating instruction, as well as the extreme position and the singular position of the redundant joint, whether the operating instruction controls the redundant joint to avoid the extreme position and the singular position.

[0081] In this step, the limit position of the redundant joint may be pre-stored in the controller. If the redundant joint moves to the limit position or even exceeds the limit position according to the operating instruction, it means that the operating instruction is abnormal. In addition, the controller may also determine whether the adjustment angle obtained during the calculation of the movement trajectory calculation formula is a singular position. Optionally, the singular position may be an operating instruction obtained when there are multiple solutions to the movement trajectory calculation formula. If the operating instruction instructs the redundant joint to move to a singular position, it means that the operating instruction is abnormal.

[0082] S204. Determine whether the operating 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 operating 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 may obtain the position of the obstacle. The obstacle may include a second basic joint that may collide with the redundant joint. Or the obstacle may also be the main body of the surgical robot, etc. The controller may determine whether the redundant joint may collide with the obstacle after the redundant joint moves according to the operating instruction based on the real-time position of the redundant joint and the operating instruction. If a collision may occur, the operating instruction is abnormal.

[0084] S205. If the operating instruction fails to control the redundant joint to avoid the extreme position or the singular position, or the operating instruction fails to control the redundant joint to avoid the second basic joint, a second alarm message is generated, wherein the second alarm message indicates that the operating 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 the operation instruction is abnormal. That is, further, the generation process of the operation instruction is abnormal. 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 may generate an obstacle avoidance effect report according to the execution results of the above steps S201 to S205, and record the result of whether the obstacle avoidance is successful.

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

[0088] S301. If the operation instruction does not control the redundant joint to avoid the extreme position or the singular position, or the operation instruction does not control the redundant joint to avoid the second basic joint, then the operation instruction is recorded.

[0089] In this step, the controller can also record the operation instruction when the alarm information is generated in the above steps S201 to S205. Optionally, the controller can also record the real-time position and real-time speed of the basic joint that generates the operation instruction, and the position of the redundant joint before and after the movement.

[0090] S302: Optimize preset parameters of the movement trajectory calculation formula according to the recorded operation 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 movement 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 results, the controller can adjust , , Three parameters to improve system stability and response speed.

[0092] Figure 2 The schematic diagram of the structure of the joint error optimization control device provided in the present application is applied to the mechanical arm of a minimally invasive robot, wherein the mechanical arm includes multiple joints, such as Figure 2 As shown, the joint error optimization control device 400 provided in this embodiment includes:

[0093] The acquisition module 401 is used 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 robot arm moves; the relative angle information represents the relative angle between the basic joint and other joints.

[0094] The processing module 402 is used to generate and execute the operation instruction of 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 movement of the basic joint.

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

[0096] Calculating a position error and a velocity error of the basic joint according to the real-time position and the real-time velocity of the basic joint;

[0097] According to the position error, the speed error, and the relative angle information of the basic joint, an operation instruction of the redundant joint is generated and executed.

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

[0099] 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;

[0100] The speed error of the basic joint is determined according to the difference between the preset target speed of the basic joint and the real-time speed of the basic joint.

[0101] Optionally, the basic joint includes 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 collision risk with the redundant joint; the processing module 402 is used to:

[0102] Determining an 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;

[0103] An operation instruction of the redundant joint is generated according to the adjustment angle of the redundant joint.

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

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

[0106] The movement trajectory calculation formula is optimized according to the relative angle information of the second basic joint, and the adjustment angle of the redundant joint is calculated according to the movement trajectory calculation formula.

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

[0108] Acquire the position of the redundant joint after executing the operation instruction and the final position indicated by the operation instruction;

[0109] If the current position does not match the final position, a first alarm message is generated, wherein the first alarm message indicates that the operation instruction response is abnormal.

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

[0111] According to the real-time position of the redundant joint and the operation instruction, as well as the extreme position and the singular position of the redundant joint, determining whether the operation instruction controls the redundant joint to avoid the extreme position and the singular position;

[0112] 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 that has a collision risk with the redundant joint;

[0113] If the operating instruction fails to control the redundant joint to avoid the extreme position or the singular position, or the operating instruction fails to control the redundant joint to avoid the second basic joint, a second alarm message is generated, and the second alarm message indicates that the operating instruction is abnormal.

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

[0115] If the operation instruction does not control the redundant joint to avoid the extreme position or the singular position, or the operation instruction does not control the redundant joint to avoid the second basic joint, then the operation instruction is recorded;

[0116] The preset parameters of the movement trajectory calculation formula are optimized according to the recorded operation instructions.

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

[0118] Figure 3 This is a schematic diagram of the structure of the controller provided in this application. Figure 3 As 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. The processor 501, the memory 502 and the communication component 503 are connected via a bus 504.

[0119] In a specific implementation process, at least one processor 501 executes the computer-executable 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 be found in the above method embodiment, and its implementation principle and technical effect are similar, so this embodiment will not be repeated here.

[0121] In the above embodiments, it should be understood that the processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), etc. A general-purpose processor may be a microprocessor or the processor may be any conventional processor, etc. The steps of the method disclosed in the invention may be directly embodied as being executed by a hardware processor, or may be executed 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 (NVM), such as at least one disk storage.

[0123] The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, the bus in the drawings of this application is 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 implements the above method when executed by a processor.

[0125] The present application also provides a computer-readable storage medium, in which computer-executable instructions are stored. When a processor executes the computer-executable instructions, the above method is implemented.

[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 static random access memory, electrically erasable programmable read-only memory, erasable programmable read-only memory, programmable read-only memory, read-only memory, magnetic memory, flash memory, magnetic disk or optical disk. The readable storage medium can be any available medium that can be accessed by a general or special-purpose computer.

[0127] An exemplary readable storage medium is coupled to a processor so that the processor can read information from the readable storage medium 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 in the device as discrete components.

[0128] The division of units is only a logical function division, and there may be other divisions in actual implementation, such as 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 mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interface, device or unit, which can be electrical, mechanical or other forms.

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

[0130] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into 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 this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, server, or network device, etc.) to perform all or part of the steps of the methods of each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc. Various media that can store program codes.

[0132] Those skilled in the art can understand that all or part of the steps of implementing the above-mentioned method embodiments can be completed by hardware related to program instructions. The aforementioned program can be stored in a computer-readable storage medium. When the program is executed, the steps of the above-mentioned method embodiments are executed; and the aforementioned storage medium includes: ROM, RAM, disk or optical disk and other media that can store program codes.

[0133] Finally, it should be noted that those skilled in the art will readily conceive of other embodiments of the present invention after considering the specification and practicing the invention disclosed herein. The present invention is intended to cover any variations, uses or adaptations of the present invention, which follow the general principles of the present invention and include common knowledge or customary technical means in the art not disclosed by the present invention, are not limited to the precise structure described above and shown in the drawings, and may be modified and changed in various ways without departing from the scope thereof. The scope of the present invention is limited only by the appended claims.

Claims

1. A joint error optimization control method, characterized in that: A mechanical arm applied to a minimally invasive robot, wherein the mechanical arm includes a plurality of basic joints and a redundant joint, and the method includes: When the robotic arm moves, the real-time position and real-time speed of the basic joint, as well as the relative angle information of the basic joint are obtained; the relative angle information represents the relative angle between the basic joint and the basic joints and redundant joints on the robotic arm other than the basic joint; According to the real-time position, the real-time speed and the relative angle information of the basic joint, an operation instruction of the redundant joint is generated and executed; wherein the operation instruction is used to control the redundant joint to follow the movement of the basic joint.

2. The method according to claim 1, characterized in that Generating and executing the operation instruction of the redundant joint according to the real-time position, the real-time speed and the relative angle information of the basic joint, including: Calculating a position error and a velocity error of the basic joint according to the real-time position and the real-time velocity of the basic joint; According to the position error, the speed error, and the relative angle information of the basic joint, an operation instruction of the redundant joint is generated and executed.

3. The method according to claim 2, characterized in that Calculating 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 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; The speed error of the basic joint is determined according to the difference between the preset target speed of the basic joint and the real-time speed of the basic joint.

4. The method according to claim 2, characterized in that: The basic joint includes 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 collision risk with the redundant joint; Generating an operation instruction of the redundant joint according to the position error, the speed error, and the relative angle information of the basic joint, including: Determining an 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; An operation instruction of the redundant joint is generated according to the adjustment angle of the redundant joint.

5. The method according to claim 4, characterized in that 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: Generate the movement trajectory calculation formula according to the position error and the speed error of the first basic joint and preset parameters of the movement trajectory calculation formula; The movement trajectory calculation formula is optimized according to the relative angle information of the second basic joint, and the adjustment angle of the redundant joint is calculated according to the movement trajectory calculation formula.

6. The method according to any one of claims 1 to 5, characterized in that The method further comprises: Obtaining the position of the redundant joint after executing the operation instruction and the final position indicated by the operation instruction; If the current position does not match the final position, a first alarm message is generated, wherein the first alarm message indicates that the operation instruction response is abnormal.

7. The method according to any one of claims 1 to 5, characterized in that The method further comprises: According to the real-time position of the redundant joint and the operation instruction, as well as the extreme position and the singular position of the redundant joint, determining whether the operation instruction controls the redundant joint to avoid the extreme position and the singular position; 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 that has a collision risk with the redundant joint; If the operating instruction fails to control the redundant joint to avoid the extreme position or the singular position, or the operating instruction fails to control the redundant joint to avoid the second basic joint, a second alarm message is generated, and the second alarm message indicates that the operating instruction is abnormal.

8. The method according to claim 7, characterized in that The method further comprises: If the operation instruction does not control the redundant joint to avoid the extreme position or the singular position, or the operation instruction does not control the redundant joint to avoid the second basic joint, then the operation instruction is recorded; The preset parameters of the movement trajectory calculation formula are optimized according to the recorded operation instructions.

9. A joint error optimization control device, characterized in that: A mechanical arm applied to a minimally invasive robot, the mechanical arm comprising a plurality of basic joints and a redundant joint, the device comprising: An acquisition module, used for acquiring 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 is used to generate and execute an operation instruction of 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 movement of the basic joint.

10. A controller, characterized in that: include: Memory, processor; The memory stores computer-executable instructions; The processor executes the computer-executable instructions stored in the memory, so that the processor performs the method according to any one of claims 1 to 8.

11. A minimally invasive robot, characterized in that: The mechanical arm of the minimally invasive robot includes redundant joints and basic joints; the minimally invasive robot is provided with a controller as shown in claim 10 for realizing error optimization of the redundant joints.

12. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-executable instructions, which are used to implement the method according to any one of claims 1 to 8 when executed by a processor.

13. A computer program product, characterized in that The invention comprises a computer program, which implements the method according to any one of claims 1 to 8 when being executed by a processor.

Images