Redundant joint follow-up obstacle avoidance method, controller and minimally invasive robot

Abstract

The embodiment of the invention provides a redundant joint follow-up obstacle avoidance method, a controller and a minimally invasive robot. The method comprises the steps that a controller can obtain joint information of a redundant joint of a mechanical arm wrist of the minimally invasive robot and joint information of a basic joint corresponding to the redundant joint; the controller can accurately calculate the optimal obstacle avoidance trajectory of the redundant joints in the obstacle avoidance process by using the joint information of the redundant joints and the joint information of the basic joints in combination with a linear function and a trigonometric function in a preset trajectory planning algorithm. The controller can generate a control instruction of the redundant joint according to the obstacle avoidance track and control the redundant joint to move. The method is used for improving the follow-up obstacle avoidance effect of redundant joints.

Description

Technical Field

[0001] The present application relates to the field of robot control, and in particular to a redundant joint follow-up obstacle avoidance method, a controller and a minimally invasive robot. Background Art

[0002] With the development of medical technology, minimally invasive surgical robots, as an important auxiliary tool for modern surgical operations, are gradually changing the face of traditional surgery. The wrist of the minimally invasive surgical robot can integrate multiple joints to achieve flexible and varied surgical movements. Among them, the introduction of redundant joints is to enhance the flexibility and adaptability of robot operations.

[0003] When the operator operates the robot arm of the minimally invasive surgical robot, the basic joint of the robot arm realizes the basic movement of the robot arm, and the redundant joint improves the flexibility of the robot arm movement by following the movement. At present, the obstacle avoidance control of the redundant joint of the robot arm wrist of the minimally invasive surgical robot is usually achieved through speed control and torque control.

[0004] However, these existing technologies still have certain limitations in implementation. Therefore, how to further improve the obstacle avoidance control of the redundant joints of the wrist of the robotic arm of the minimally invasive surgical robot is still a problem that needs to be explored. Summary of the invention

[0005] The embodiments of the present application provide a redundant joint follow-up obstacle avoidance method, a controller and a minimally invasive robot to improve the follow-up obstacle avoidance effect of the redundant joints.

[0006] In a first aspect, an embodiment of the present application provides a redundant joint follow-up obstacle avoidance method, which is applied to a minimally invasive robot, wherein the wrist of a mechanical arm of a main hand of the minimally invasive robot includes a redundant joint and a basic joint, including:

[0007] Acquire joint information of the basic joints and the redundant joints of the wrist of the robotic arm of the minimally invasive robot;

[0008] According to the joint information of the redundant joint and the joint information of the basic joint, an obstacle avoidance trajectory of the redundant joint is planned by a trigonometric function trajectory planning algorithm;

[0009] The redundant joint movement is controlled according to the obstacle avoidance trajectory.

[0010] Optionally, planning the obstacle avoidance trajectory of the redundant joint according to the joint information of the redundant joint and the joint information of the basic joint by using a trigonometric function trajectory planning algorithm includes:

[0011] Determine the zero position and the moving trajectory of the obstacle avoidance trajectory according to the acquired position information of the operator's hand and the position information in the joint information of the basic joint; the moving trajectory represents the trajectory of the redundant joint moving from the current position to the zero position;

[0012] Based on the joint angles in the joint information of the basic joints and the zero position of the obstacle avoidance trajectory, the obstacle avoidance trajectory of the redundant joints is generated through a trigonometric function trajectory planning algorithm.

[0013] Optionally, determining the zero position and the moving trajectory of the obstacle avoidance trajectory according to the position information of the operator's hand and the position information in the joint information of the basic joints includes:

[0014] Using the intermediate position between the acquired hand position of the operator and the joint position of the basic joint as the zero position of the obstacle avoidance trajectory;

[0015] The uniform motion straight line between the current position and the zero position of the redundant joint is used as the movement trajectory.

[0016] Optionally, based on the joint angle in the joint information of the basic joint and the zero position of the obstacle avoidance trajectory, the obstacle avoidance trajectory of the redundant joint is generated by a trigonometric function trajectory planning algorithm, including:

[0017] determining an obstacle avoidance direction of the redundant joint according to the joint angle of the basic joint;

[0018] Determining an end point of the obstacle avoidance trajectory according to the joint angle in the joint information of the basic joint;

[0019] Generate a path of the obstacle avoidance trajectory according to the obstacle avoidance direction and the end point;

[0020] The obstacle avoidance trajectory is generated according to the trigonometric function trajectory planning algorithm and the path.

[0021] Optionally, generating the obstacle avoidance trajectory according to the trigonometric function trajectory planning algorithm and the path includes:

[0022] generating a trajectory equation according to the path;

[0023] According to the preset time interval, a plurality of trajectory points are calculated by using the trajectory equation;

[0024] The plurality of trajectory points are smoothly processed to obtain the obstacle avoidance trajectory.

[0025] Optionally, the method further includes:

[0026] Adjusting the speed range and movement range of the redundant joints in the obstacle avoidance trajectory according to a preset coefficient;

[0027] The obstacle avoidance trajectory of the redundant joint is optimized according to the speed range and the movement range of the redundant joint.

[0028] Optionally, controlling the redundant joint movement according to the obstacle avoidance trajectory includes:

[0029] According to preset parameters, adjusting the response rate and allowable error of the redundant joint;

[0030] generating control instructions for the redundant joints according to the obstacle avoidance trajectory;

[0031] The control instruction is responded to according to the adjusted response rate, and the execution of the control instruction is monitored according to the allowable error.

[0032] Optionally, the method further includes:

[0033] If the error when the control instruction is executed is greater than or equal to the allowable error, an abnormal alarm is generated, and the abnormal alarm indicates that the movement error of the redundant joint is too large.

[0034] In a second aspect, an embodiment of the present application provides a redundant joint follow-up obstacle avoidance device, which is applied to a minimally invasive robot, wherein the wrist of a mechanical arm of a main hand of the minimally invasive robot includes a redundant joint and a basic joint, including:

[0035] An acquisition module, used to acquire joint information of the basic joints and the redundant joints of the wrist of the robotic arm of the minimally invasive robot;

[0036] The processing module is used to plan the obstacle avoidance trajectory of the redundant joint according to the joint information of the redundant joint and the joint information of the basic joint through a trigonometric function trajectory planning algorithm; and control the movement of the redundant joint according to the obstacle avoidance trajectory.

[0037] Optionally, the processing module is used to:

[0038] Determine the zero position and the moving trajectory of the obstacle avoidance trajectory according to the acquired position information of the operator's hand and the position information in the joint information of the basic joint; the moving trajectory represents the trajectory of the redundant joint moving from the current position to the zero position;

[0039] Based on the joint angles in the joint information of the basic joints and the zero position of the obstacle avoidance trajectory, the obstacle avoidance trajectory of the redundant joints is generated through a trigonometric function trajectory planning algorithm.

[0040] Optionally, the processing module is used to:

[0041] Using the intermediate position between the acquired hand position of the operator and the joint position of the basic joint as the zero position of the obstacle avoidance trajectory;

[0042] The uniform motion straight line between the current position and the zero position of the redundant joint is used as the movement trajectory.

[0043] Optionally, the processing module is used to:

[0044] determining an obstacle avoidance direction of the redundant joint according to the joint angle of the basic joint;

[0045] Determining an end point of the obstacle avoidance trajectory according to the joint angle in the joint information of the basic joint;

[0046] Generate a path of the obstacle avoidance trajectory according to the obstacle avoidance direction and the end point;

[0047] The obstacle avoidance trajectory is generated according to the trigonometric function trajectory planning algorithm and the path.

[0048] Optionally, the processing module is used to:

[0049] generating a trajectory equation according to the path;

[0050] According to the preset time interval, a plurality of trajectory points are calculated by using the trajectory equation;

[0051] The plurality of trajectory points are smoothly processed to obtain the obstacle avoidance trajectory.

[0052] Optionally, the processing module is used to:

[0053] Adjusting the speed range and movement range of the redundant joints in the obstacle avoidance trajectory according to a preset coefficient;

[0054] The obstacle avoidance trajectory of the redundant joint is optimized according to the speed range and the movement range of the redundant joint.

[0055] Optionally, the processing module is used to:

[0056] According to preset parameters, adjusting the response rate and allowable error of the redundant joint;

[0057] generating control instructions for the redundant joints according to the obstacle avoidance trajectory;

[0058] The control instruction is responded to according to the adjusted response rate, and the execution of the control instruction is monitored according to the allowable error.

[0059] Optionally, the processing module is used to:

[0060] If the error when the control instruction is executed is greater than or equal to the allowable error, an abnormal alarm is generated, and the abnormal alarm indicates that the movement error of the redundant joint is too large.

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

[0062] The memory stores computer-executable instructions;

[0063] 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.

[0064] In a fourth aspect, an embodiment of the present application provides a minimally invasive robot, characterized in that the wrist of the robotic arm of the minimally invasive robot is provided with redundant joints and basic joints; the minimally invasive robot is provided with the above third aspect and / or various possible controllers of the third aspect, which are used to realize the follow-up obstacle avoidance of the redundant joints of the robotic arm.

[0065] 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.

[0066] 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.

[0067] The redundant joint follow-up obstacle avoidance method, controller and minimally invasive robot provided in the embodiments of the present application obtain the joint information of the redundant joints at the wrist of the mechanical arm of the minimally invasive robot, and the joint information of the basic joints corresponding to the redundant joints; use the joint information of the redundant joints and the joint information of the basic joints, and combine the linear function and trigonometric function in the preset trajectory planning algorithm to accurately calculate the optimal obstacle avoidance trajectory of the redundant joints in the obstacle avoidance process; according to the obstacle avoidance trajectory, generate control instructions for the redundant joints, and control the means of movement of the redundant joints to achieve the effect of follow-up obstacle avoidance of the redundant joints. Furthermore, the improvement of the follow-up obstacle avoidance effect of the redundant joints can improve the doctor's operating flexibility on the main hand, and avoid the doctor's hand and other joints from interfering with the redundant joints, causing abnormal shaking of the main hand. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] 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.

[0069] Figure 1 A schematic diagram of the flow chart of the redundant joint follow-up obstacle avoidance method provided in this application;

[0070] Figure 2A schematic diagram of the structure of the redundant joint follow-up obstacle avoidance device provided in this application;

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

[0072] 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

[0073] 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.

[0074] Minimally invasive surgical robots usually include a master hand and a slave hand. The wrist of the master hand's robotic arm is generally composed of four joints. Among the four joints, there is a redundant joint. The setting of the redundant joint is used to ensure that the doctor's operation is flexible and smooth during normal surgery. The movement of the redundant joint needs to avoid interference with other joints and the doctor's hand. That is, the obstacle avoidance function of the redundant joint needs to be realized.

[0075] At present, obstacle avoidance of redundant joints can be completed in speed control and torque control modes. Among them, speed control requires the controlled driver to work in speed mode. The redundant joint can achieve the purpose of obstacle avoidance by real-time planning of the joint speed through the host computer. The obstacle avoidance algorithm of the speed control needs to output accurate speed during the planning process. If there is a more complex transmission mechanism between the motor driving the redundant joint and the redundant joint, the redundant joint may have jitter and avoidance. Among them, torque control is more complicated because it not only needs to consider the path planning problem but also the influence of gravity changes on torque output. The obstacle avoidance algorithm of torque control is more suitable for occasions where the gravity and direction of the redundant joints remain unchanged. Changing gravity will increase the planning difficulty in the torque control process, and the control accuracy will also be reduced accordingly. In addition, position control is also a commonly used obstacle avoidance method in the prior art. However, the obstacle avoidance trajectory planning control algorithm of the position mode may have the problem of interference caused by inappropriate obstacle avoidance angle.

[0076] In order to further improve the obstacle avoidance effect of redundant joints, the present application proposes a redundant joint follow-up obstacle avoidance method based on trigonometric function trajectory planning. The present application effectively avoids the above-mentioned drawbacks by using trigonometric functions to plan the path of the redundant joints in the position mode, and setting parameters to adjust the speed range and movement range of the planned trajectory. Specifically, the present application can divide the redundant joint follow-up obstacle avoidance method into two parts. Among them, the first part can plan the obstacle avoidance path of the redundant joints by using linear functions and trigonometric functions. During the path planning process, the present application can also adjust the speed range and movement range of trajectory planning by calling the reserved parameter interface, effectively avoiding the drawbacks of the above method. The second part is the adjustment layer of trajectory planning, whose main function is to adjust the response rate and allowable error of trajectory tracking and other parameters. The present application reserves the setting parameters in this part for adjusting the trajectory running state, and finally sends the real-time planned trajectory to the motor of the redundant joint in the position mode, effectively avoiding problems such as joint jitter, mutual interference, and gravity influence. The implementation of the present application timely avoids the interference of the operator's hands and other joints during the movement of the redundant joints, and ensures the smoothness and flexibility of the redundant joints during operation, reduces jitter, and makes the operation more flexible and smooth.

[0077] 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.

[0078] Figure 1 A flow chart of the redundant joint follow-up obstacle avoidance method provided in the present application. The redundant joint follow-up obstacle avoidance method is applied to a minimally invasive robot. The wrist of the robotic arm of the main hand of the minimally invasive robot includes a redundant joint, and the redundant joint is associated with a basic joint of the wrist of the robotic arm. When the operator operates the main hand robotic arm, the redundant joint moves with the movement of the basic joint. And the angle between the redundant joint and the basic joint is always close to 90 degrees to avoid collision with the basic joint. The present application uses a controller as the execution subject, such as Figure 1 As shown, the method includes:

[0079] S101. Acquire joint information of basic joints and redundant joints of a robotic arm wrist of a minimally invasive robot.

[0080] In this embodiment, the controller first obtains the joint information of the redundant joint of the wrist of the minimally invasive robot, and the joint information of the basic joint associated with the redundant joint. Optionally, the joint information may include information such as joint angle, joint position and joint motion range. Optionally, the controller may obtain the joint information of each joint in real time through sensors and encoders.

[0081] S102: According to the joint information of the redundant joint and the joint information of the basic joint, an obstacle avoidance trajectory of the redundant joint is planned by a preset planning algorithm, wherein the preset planning algorithm is determined according to a linear function and a trigonometric function.

[0082] In this embodiment, the controller can use the joint information of the redundant joints and the joint information of the basic joints, and combine with the preset trajectory planning algorithm determined based on a linear function and a trigonometric function to accurately calculate the optimal obstacle avoidance trajectory of the redundant joints during the obstacle avoidance process. Among them, the application of trigonometric functions in the preset trajectory planning algorithm can fully consider the joint position, joint angle, joint motion range and other information of each joint during the trajectory planning process to ensure that the obstacle avoidance trajectory is both safe and efficient.

[0083] In one example, the planning process of the obstacle avoidance trajectory can be divided into the following two steps:

[0084] Step 1: According to the acquired position information of the operator's hand and the current position in the joint information of the redundant joint, a zero position and a first obstacle avoidance trajectory are determined through a preset planning algorithm; the first obstacle avoidance trajectory represents the trajectory of the redundant joint moving from the current position to the zero position.

[0085] In this step, the controller can also obtain the position information of the operator's hand through the sensor. The controller can determine the zero position of the obstacle avoidance trajectory of the redundant joint based on the position information of the hand and the current position in the joint information of the redundant joint. The setting of the zero position can avoid the interference between the redundant joint and the operator's hand and other basic joints as much as possible. After determining the zero position, the controller can determine the first obstacle avoidance trajectory of the redundant joint moving to the zero position through a preset planning algorithm. Optionally, the first obstacle avoidance trajectory can be the first part of the obstacle avoidance trajectory of step S101 above.

[0086] Specifically, the specific process of the controller determining the zero position of the obstacle avoidance trajectory and the first obstacle avoidance trajectory may include:

[0087] Step 11: The mutually perpendicular and orthogonal position of the acquired hand position of the operator and the current position of the redundant joint is used as the zero position of the obstacle avoidance trajectory.

[0088] In this step, the controller may calculate the intermediate position between the hand position and the current position of the redundant joint in three-dimensional space. Specifically, the intermediate position may be a mutually perpendicular or orthogonal position of the hand position and the current position of the redundant joint. The controller may use the intermediate position as the zero position. Optionally, the hand position and the joint position may be three-dimensional coordinate points. Optionally, the coordinate system of the three-dimensional coordinate points may be a coordinate system determined according to the master hand.

[0089] Step 12: According to a linear function in a preset planning algorithm, a linear motion trajectory between the current position and the zero position of the redundant joint is determined, and the linear motion trajectory is used as the first obstacle avoidance trajectory.

[0090] In this step, the controller can use a linear function in a preset planning algorithm to generate a straight line connecting the current position and the zero position as a moving path. The controller can set the movement of the redundant joint to move at a first speed at a uniform speed. The controller generates the first obstacle avoidance trajectory according to the first speed and the moving path. The first obstacle avoidance trajectory may include multiple trajectory points at fixed time intervals. The moving trajectory is equivalent to a uniform linear function trajectory.

[0091] Step 2: Based on the joint angles in the joint information of the basic joint and the zero position of the obstacle avoidance trajectory, a second obstacle avoidance trajectory of the redundant joint is generated through a preset planning algorithm; the second obstacle avoidance trajectory represents the trajectory of the redundant joint moving from the zero position to the obstacle avoidance target position.

[0092] In this step, the controller can analyze the joint angles in the joint information of each basic joint to determine the trajectory direction of the basic joint and the obstacle avoidance direction of the redundant joint. Furthermore, the controller can determine the optimal target position for obstacle avoidance based on the joint positions in the joint information of each basic joint and the trajectory direction. The target position for obstacle avoidance is the end point of the obstacle avoidance trajectory. The controller can generate a second obstacle avoidance trajectory for the redundant joint from the zero position to the end point of the trajectory according to the trajectory direction through the trigonometric functions in the preset planning algorithm. Optionally, the second obstacle avoidance trajectory can be the second part of the obstacle avoidance trajectory indicated in step S102. The first obstacle avoidance trajectory is combined with the second obstacle avoidance estimate to obtain a complete obstacle avoidance trajectory.

[0093] Specifically, the process of the controller generating the second obstacle avoidance trajectory of the redundant joint may include:

[0094] Step 21: Determine the obstacle avoidance direction of the redundant joint according to the joint angle of the basic joint.

[0095] In this step, the controller can first determine the obstacle avoidance direction that the redundant joint needs to take through calculation and analysis based on the joint angle of the basic joint. When the redundant joint moves based on the obstacle avoidance direction, the redundant joint is highly likely not to conflict with the operator's operation direction, thereby avoiding the redundant joint from interfering with the operator's operation.

[0096] Step 22: Determine the end point of the obstacle avoidance trajectory according to the joint angle in the joint information of the basic joint.

[0097] In this step, the controller can calculate the moving end point of the redundant joint according to the joint angle in the joint information of the basic joint. The moving end point is the end point of the obstacle avoidance trajectory. At the obstacle avoidance end point, the redundant joint realizes the follow-up movement with the basic joint. And, at the obstacle avoidance end point, the redundant joint can avoid interfering with the operator's operation to the greatest extent, and perform subsequent movement along with the operator's operation.

[0098] Step 23: Generate an obstacle avoidance trajectory path according to the obstacle avoidance direction and the end point.

[0099] In this step, the controller can further plan the specific path of the obstacle avoidance trajectory after determining the obstacle avoidance direction, zero position and end point.

[0100] Step 24: Calculate the path according to the trigonometric function in the preset planning algorithm to obtain the second obstacle avoidance trajectory of the redundant joint.

[0101] In this step, the controller can use the trigonometric function trajectory planning algorithm, combined with the determined path, to generate a smooth and efficient obstacle avoidance trajectory. Optionally, the obstacle avoidance trajectory generation process may include:

[0102] Step 241: Process the path according to trigonometric functions to generate a trajectory equation.

[0103] In this step, after determining the obstacle avoidance path, the controller uses a trigonometric function trajectory planning algorithm to generate a trajectory equation, which may be preset with information such as the speed and acceleration of the redundant joint.

[0104] Step 242: Calculate the trajectory point at the next moment in real time using the trajectory equation according to the preset time interval.

[0105] In this step, a time interval may be preset in the controller. The time interval may be determined according to the interval of control instructions during the movement of the redundant joints of the controller. Alternatively, the time interval may be determined according to the allowable delay during the movement of the redundant joints of the controller. The controller may calculate the trajectory point at the next moment according to the time interval by using the trajectory equation. Optionally, the calculated multiple trajectory points are discrete points. Optionally, during the movement of the redundant joint, the controller may cross-execute the process of calculating the trajectory point at the next moment and controlling the movement of the redundant joint in the process of controlling the movement of the redundant joint.

[0106] Step 243: Smooth the trajectory point and generate a control signal for moving to the trajectory point.

[0107] In this step, after calculating the trajectory point at the next moment, the controller can perform smoothing based on the current position and trajectory point of the redundant joint so that the redundant joint can move smoothly to the trajectory point. Based on the line between the current position and the trajectory point, the controller can obtain a second obstacle avoidance trajectory through a smoothing algorithm to improve the smoothing effect of the redundant joint during movement. For example, the controller can use a filtering algorithm or a curve fitting technique to eliminate abrupt turns or jitters in the general outline of the obstacle avoidance trajectory.

[0108] Optionally, after calculating the obstacle avoidance trajectory in step 243, the controller may further optimize the obstacle avoidance trajectory through the following steps. The process may include:

[0109] Step 244: Adjust the speed range and movement range of the redundant joints in the obstacle avoidance trajectory according to the preset coefficients.

[0110] In this step, the controller may store a plurality of preset coefficients of the redundant joint. Optionally, the preset coefficients may include the maximum joint angle, the minimum joint angle, the maximum moving speed, the minimum moving speed, etc. of the redundant joint. The controller may calculate the speed range and the moving range of the redundant joint in the obstacle avoidance trajectory based on the preset coefficients of the redundant joint.

[0111] Step 245: Optimize the obstacle avoidance trajectory of the redundant joints according to the speed range and movement range of the redundant joints.

[0112] In this step, the controller can determine whether the speed of the redundant joint in the obstacle avoidance trajectory is within the speed range and whether the position is within the movement range based on the speed range and the movement range. If not, the controller can adjust the speed and movement direction in the obstacle avoidance trajectory based on the speed range and the movement range to obtain an optimized obstacle avoidance trajectory.

[0113] S103 . Determine a control signal of the redundant joint according to the obstacle avoidance trajectory, and control the movement of the redundant joint according to the control signal.

[0114] In this embodiment, the controller can generate control instructions according to the planned obstacle avoidance trajectory, and control the movement of the redundant joint to achieve follow-up obstacle avoidance of the redundant joint, thereby improving the doctor's operating flexibility in the main hand and avoiding the doctor's hand and other joints from interfering with the redundant joint, resulting in abnormal shaking of the main hand. Optionally, the controller can obtain the control signal of the redundant joint after adjusting the obstacle avoidance trajectory through PID.

[0115] In one example, the specific process of the controller controlling the redundant joint movement may include:

[0116] Step 1: Adjust the response rate and allowable error of the redundant joints according to the preset parameters.

[0117] In this step, the controller can adjust the response rate, allowable error and other parameters of the redundant joints through PID according to preset parameters.

[0118] Step 2: Generate control instructions for redundant joints based on the obstacle avoidance trajectory.

[0119] In this step, the controller can generate a control instruction for the redundant joint based on the track point in the obstacle avoidance track. The control instruction can be used to control the torque of the motor of the redundant joint.

[0120] Step 3: respond to the control instruction according to the adjusted response rate, and monitor the execution of the control instruction according to the allowable error.

[0121] In this step, the controller can respond to the control instruction according to the adjusted response rate to ensure the response speed of the redundant joint, so that the movement of the redundant joint is smooth and stable. At the same time, the controller can also monitor the execution of the control instruction according to the allowable error to ensure that the redundant joint moves in place based on the control instruction, thereby ensuring the movement stability of the redundant joint and further reducing the interference between the redundant joint and the hand or other joints.

[0122] Optionally, the controller may also generate an abnormality alarm when the error during execution of the control instruction is greater than or equal to the allowable error. The abnormality alarm indicates that the movement error of the redundant joint is too large. The abnormality alarm is used to remind the operator to pay attention to interference.

[0123] The redundant joint follow-up obstacle avoidance method provided by the embodiment of the present application, wherein the wrist of the mechanical arm of the minimally invasive robot includes a redundant joint and a basic joint corresponding to the redundant joint. After obtaining the joint information of the basic joint, the joint information of the redundant joint, and the hand information of the operator; and the three information, the linear function and trigonometric function in the preset planning algorithm can be used to plan the obstacle avoidance trajectory; according to the obstacle avoidance trajectory, the control signal of the redundant joint can be generated, and the movement of the redundant joint can be controlled; thereby, the follow-up obstacle avoidance of the redundant joint is realized during the movement of the basic joint, and the flexibility in the follow-up obstacle avoidance process is improved, and abnormal shaking is further avoided, thereby improving the flexibility and stability of the basic joint.

[0124] Figure 2 The schematic diagram of the structure of the redundant joint follow-up obstacle avoidance device provided in this application is as follows: Figure 2 As shown, the redundant joint follow-up obstacle avoidance device 200 provided in this embodiment includes:

[0125] The acquisition module 201 is used to acquire the joint information of the basic joints and redundant joints of the wrist of the robotic arm of the minimally invasive robot.

[0126] The processing module 202 is used to plan the obstacle avoidance trajectory of the redundant joints according to the joint information of the redundant joints and the joint information of the basic joints through a preset planning algorithm. The preset planning algorithm is determined according to a linear function and a trigonometric function. The control signal of the redundant joint is determined according to the obstacle avoidance trajectory, and the movement of the redundant joint is controlled according to the control signal.

[0127] Optionally, the processing module 202 is configured to:

[0128] According to the acquired position information of the operator's hand and the current position in the joint information of the redundant joint, a zero position and a first obstacle avoidance trajectory of the obstacle avoidance trajectory are determined through a preset planning algorithm. The first obstacle avoidance trajectory represents a trajectory of the redundant joint moving from the current position to the zero position.

[0129] Based on the joint angles in the joint information of the basic joints and the zero position of the obstacle avoidance trajectory, a second obstacle avoidance trajectory of the redundant joint is generated through a preset planning algorithm. The second obstacle avoidance trajectory represents the trajectory of the redundant joint moving from the zero position to the obstacle avoidance target position.

[0130] Optionally, the processing module 202 is configured to:

[0131] The mutually perpendicular and orthogonal position of the acquired hand position of the operator and the current position of the redundant joint is taken as the zero position of the obstacle avoidance trajectory.

[0132] According to a linear function in a preset planning algorithm, a linear motion trajectory between the current position and the zero position of the redundant joint is determined, and the linear motion trajectory is used as the first obstacle avoidance trajectory.

[0133] Optionally, the processing module 202 is configured to:

[0134] According to the joint angles of the basic joints, the obstacle avoidance directions of the redundant joints are determined.

[0135] The end point of the obstacle avoidance trajectory is determined according to the joint angles in the joint information of the basic joints.

[0136] Generate the path of the obstacle avoidance trajectory based on the obstacle avoidance direction and end point.

[0137] According to the trigonometric function in the preset planning algorithm, the path is calculated to obtain the second obstacle avoidance trajectory of the redundant joint.

[0138] Optionally, the processing module 202 is configured to:

[0139] The path is processed according to trigonometric functions to generate the trajectory equation.

[0140] According to the preset time interval, the trajectory point at the next moment is calculated in real time through the trajectory equation.

[0141] Smooth the second obstacle avoidance trajectory from the current position of the redundant joint to the trajectory point.

[0142] Optionally, the processing module 202 is configured to:

[0143] The speed range and movement range of redundant joints in the obstacle avoidance trajectory are adjusted according to the preset coefficients.

[0144] The obstacle avoidance trajectory of the redundant joints is optimized according to the speed range and movement range of the redundant joints.

[0145] Optionally, the processing module 202 is configured to:

[0146] Adjust the response rate and allowable error of redundant joints according to preset parameters.

[0147] Generate control instructions for redundant joints based on obstacle avoidance trajectories.

[0148] Respond to control commands according to the adjusted response rate and monitor control command execution according to the allowable error.

[0149] Optionally, the processing module 202 is configured to:

[0150] If the error when the control instruction is executed is greater than or equal to the allowable error, an abnormal alarm is generated, and the abnormal alarm indicates that the movement error of the redundant joint is too large.

[0151] The redundant joint follow-up obstacle avoidance 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.

[0152] Figure 3 This is a schematic diagram of the structure of the controller provided in this application. Figure 3 As shown, the electronic device 30 provided in this embodiment includes: at least one processor 301 and a memory 302. Optionally, the device 30 also includes a communication component 303. The processor 301, the memory 302 and the communication component 303 are connected via a bus 304.

[0153] In a specific implementation process, at least one processor 301 executes the computer-executable instructions stored in the memory 302, so that at least one processor 301 executes the above method.

[0154] The specific implementation process of the processor 301 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.

[0155] In the above embodiments, it should be understood that the processor can 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 can be a microprocessor or any conventional processor. The steps of the method disclosed in the invention can be directly implemented as a hardware processor, or can be implemented by a combination of hardware and software modules in the processor.

[0156] 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.

[0157] 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.

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

[0159] 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.

[0160] 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 (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), 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.

[0161] 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 (Application Specific Integrated Circuits, referred to as: ASIC). Of course, the processor and the readable storage medium can also exist in the device as discrete components.

[0162] 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.

[0163] 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.

[0164] 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.

[0165] 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.

[0166] 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.

[0167] 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 redundant joint follow-up obstacle avoidance method, characterized in that: Applied to a minimally invasive robot, the wrist of a mechanical arm of a main hand of the minimally invasive robot includes a redundant joint and a basic joint, and the method includes: Acquire joint information of the basic joints and the redundant joints of the wrist of the robotic arm of the minimally invasive robot; According to the joint information of the redundant joint and the joint information of the basic joint, an obstacle avoidance trajectory of the redundant joint is planned by a preset planning algorithm; wherein the preset planning algorithm is determined according to a linear function and a trigonometric function; A control signal of the redundant joint is determined according to the obstacle avoidance trajectory, and the movement of the redundant joint is controlled according to the control signal.

2. The method according to claim 1, characterized in that: According to the joint information of the redundant joint and the joint information of the basic joint, an obstacle avoidance trajectory of the redundant joint is planned by a preset planning algorithm, including: According to the acquired position information of the operator's hand and the current position in the joint information of the redundant joint, the zero position and the first obstacle avoidance trajectory of the obstacle avoidance trajectory are determined by the preset planning algorithm; the first obstacle avoidance trajectory represents the trajectory of the redundant joint moving from the current position to the zero position; Based on the joint angles in the joint information of the basic joint and the zero position of the obstacle avoidance trajectory, a second obstacle avoidance trajectory of the redundant joint is generated through the preset planning algorithm; the second obstacle avoidance trajectory represents the trajectory of the redundant joint moving from the zero position to the obstacle avoidance target position.

3. The method according to claim 2, characterized in that According to the acquired position information of the operator's hand and the current position in the joint information of the redundant joint, the zero position and the first obstacle avoidance trajectory of the obstacle avoidance trajectory are determined by the preset planning algorithm, including: The mutually perpendicular and orthogonal position of the acquired hand position of the operator and the current position of the redundant joint is used as the zero position of the obstacle avoidance trajectory; According to a linear function in the preset planning algorithm, a linear motion trajectory between the current position of the redundant joint and the zero position is determined, and the linear motion trajectory is used as the first obstacle avoidance trajectory.

4. The method according to claim 2, characterized in that: Based on the joint angle in the joint information of the basic joint and the zero position of the obstacle avoidance trajectory, a second obstacle avoidance trajectory of the redundant joint is generated by the preset planning algorithm, including: determining an obstacle avoidance direction of the redundant joint according to the joint angle of the basic joint; Determining an end point of the obstacle avoidance trajectory according to the joint angle in the joint information of the basic joint; Generate a path of the obstacle avoidance trajectory according to the obstacle avoidance direction and the end point; The path is calculated according to the trigonometric function in the preset planning algorithm to obtain a second obstacle avoidance trajectory of the redundant joint.

5. The method according to claim 4, characterized in that The path is calculated according to the trigonometric function in the preset planning algorithm to obtain a second obstacle avoidance trajectory of the redundant joint, including: Processing the path according to trigonometric functions to generate a trajectory equation; According to the preset time interval, the trajectory point at the next moment is calculated in real time by using the trajectory equation; A second obstacle avoidance trajectory from the current position of the redundant joint to the trajectory point is smoothly processed.

6. The method according to claim 5, characterized in that The method further comprises: Adjusting the speed range and movement range of the redundant joints in the obstacle avoidance trajectory according to a preset coefficient; The obstacle avoidance trajectory of the redundant joint is optimized according to the speed range and the movement range of the redundant joint.

7. The method according to any one of claims 1 to 6, characterized in that According to the obstacle avoidance trajectory, controlling the redundant joint movement includes: According to preset parameters, adjusting the response rate and allowable error of the redundant joint; generating control instructions for the redundant joints according to the obstacle avoidance trajectory; The control instruction is responded to according to the adjusted response rate, and the execution of the control instruction is monitored according to the allowable error.

8. The method according to claim 7, characterized in that The method further comprises: If the error when the control instruction is executed is greater than or equal to the allowable error, an abnormal alarm is generated, and the abnormal alarm indicates that the movement error of the redundant joint is too large.

9. A redundant joint follow-up obstacle avoidance device, characterized in that: Applied to a minimally invasive robot, the wrist of the robot arm of the minimally invasive robot includes a redundant joint and a basic joint, and the device includes: An acquisition module, used to acquire joint information of the basic joints and the redundant joints of the wrist of the robotic arm of the minimally invasive robot; The processing module is used to plan the obstacle avoidance trajectory of the redundant joint according to the joint information of the redundant joint and the joint information of the basic joint through a trigonometric function trajectory planning algorithm; and control the movement of the redundant joint according to the obstacle avoidance trajectory.

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 wrist of the robotic arm of the minimally invasive robot is provided with redundant joints and basic joints; the minimally invasive robot is provided with a controller as shown in claim 10, which is used to achieve follow-up obstacle avoidance of the redundant joints of the robotic arm.

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