Retaining method for yielding with the end pose fixed

Abstract

The present invention discloses a method for making way while keeping the tool pose fixed, including the steps of: establishing a kinematic model of the way-making arm; obtaining the constrained pose and unconstrained pose of the end of the way-making arm during the way-making process according to the way-making requirements, and constructing a virtual axis for driving the joints corresponding to its redundant degrees of freedom based on the redundant degrees of freedom obtained from the degrees of freedom of movement of the way-making arm and the degrees of freedom corresponding to its constrained pose; calculating the positions of the actual joints of the way-making arm driven by the virtual axis, and driving the corresponding joints of the way-making arm to move accordingly. The present invention constructs a constrained virtual axis to ensure the uniformity of the virtual axis speed during the way-making process, avoiding the problem that the speeds of other joints change too much caused by joint driving.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and in particular to a retracting method for keeping the end pose fixed. Background Art

[0002] A robot-assisted surgical system generally includes a scope-holding arm equipped with an endoscope and a plurality of instrument arms equipped with surgical instruments. During robot-assisted surgery, a doctor remotely controls the actions of each arm by controlling a master hand in a master-slave manner to perform the surgery. Relative movement occurs between the arms in the system during the surgery. When the initial positions of the arms are not placed reasonably, it will cause mutual collision between the arms. Therefore, it is necessary to adjust the poses of the arms before the surgery. Usually, it is necessary to ensure that the specified pose at the end of the arm member is fixed, and then close and position the arm members and separate them by a certain distance to ensure sufficient surgical space for each arm to work in parallel and avoid possible collisions between the arm members.

[0003] Existing methods use a specified joint of the arm member as the driving source, but there are significant differences in the control quantity ratios of the moving joint and the rotating joint during uniform retraction while keeping the end pose fixed. If one of the joints is used as the uniform drive, the speeds of the other joints may be too large or too small. For example, when multiple linkages of the arm member are parallel or perpendicular, the configuration will approach a singular pose. When the driving joint moves uniformly, it will cause the speeds of the other joints to change too little or too much, posing a risk of loss of control. Summary of the Invention

[0004] Object of the Invention: Aiming at the deficiencies, the present invention proposes a retracting method for keeping the end pose fixed, which maintains the speed uniformity during the retracting process, has a high end pose accuracy, and avoids the problem of excessive speed changes of other joints caused by joint driving.

[0005] Technical Solution:

[0006] A retracting method for keeping the end pose fixed includes the steps of:

[0007] Establish a kinematic model of the retracting arm;

[0008] Determine the constrained pose and the unconstrained pose of the end of the retracting arm during the retracting process, and construct a virtual axis for driving one or more degrees of freedom corresponding joints in the unconstrained pose;

[0009] Calculate the target positions of the actual joints of the retracting arm under the drive of the virtual axis, and drive the corresponding joints of the retracting arm to move accordingly.

[0010] According to the degrees of freedom that need to be kept fixed at the end of the retracting arm, obtain the constrained pose and the unconstrained pose of the end of the retracting arm during the retracting process.

[0011] The number of degrees of freedom of the virtual axis is the difference between the degrees of freedom of the movement of the yield arm and the number of degrees of freedom of the end thereof that needs to remain fixed.

[0012] Also includes the steps:

[0013] After driving the corresponding joint movement of the yielding arm, the actual posture of the end of the yielding arm is calculated, and the error between the actual posture and the constrained posture of the end of the yielding arm is compared, and the control of the corresponding joint movement is stopped when the error between the two exceeds a threshold.

[0014] The error between the actual posture of the end of the yielding arm and the posture of each corresponding degree of freedom of the constrained posture is calculated. If the error between the postures of each corresponding degree of freedom of the two is within a threshold range, a driver is sent to control the corresponding joint movement; otherwise, the control of the joint movement is stopped.

[0015] The threshold range is specifically: the translational degree of freedom is set to 0.1 mm, and the rotational degree of freedom is set to 0.05 rad.

[0016] The target positions of the actual joints of the yield arm driven by the virtual axis are calculated as follows:

[0017] Constructing a set of constraint equations for the joint positions of the yielding arm and the constrained posture of the end of the yielding arm driven by the virtual axis;

[0018] The target position of each joint of the yield arm is obtained by solving the constraint equations, and the corresponding joint motion is controlled accordingly.

[0019] After constructing the virtual axis, determine whether there is a constraint relationship between the virtual axis and the actual joints of the yield arm. If not, reselect the joints corresponding to other degrees of freedom in the non-constrained posture to construct the virtual axis.

[0020] Construct virtual axis θ v Specifically:

[0021] θ v =θ0-kθ k t

[0022] Among them, θ0 is the initial value of the degree of freedom joint selected in the unconstrained pose; θ k is the rotation angle per unit time, t is the driving time, and k is the driving ratio between the virtual axis and the degree of freedom joint to be driven.

[0023] Compared with the prior art, the present invention has the following notable advantages:

[0024] 1. Movement uniformity: Compared with the method of using the joints of the yielding arm as the driving method, the present invention constructs a constrained virtual axis to ensure the uniformity of the virtual axis speed during the yielding process, thereby avoiding the problem of excessive changes in the speed of other joints caused by joint driving.

[0025] 2. Maintain high terminal posture accuracy: Compared with the inverse Jacobian determinant solution method based on posture error, the present invention adopts an analytical method or a numerical method, and its accuracy is not affected by the posture error or the size of the velocity differential. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 This is a schematic diagram of the yield arm before yielding;

[0027] Figure 2 A flow chart of a method for making way for keeping the terminal position fixed according to the present invention;

[0028] Figure 3 for Figure 1 Flowchart of construction of virtual axis based on unconstrained pose of the end;

[0029] Figure 4 for Figure 1 Flow chart of actual joint compensation driven by virtual axis;

[0030] Figure 5 Schematic diagram of the rear give way arm. DETAILED DESCRIPTION

[0031] The present invention is further explained below in conjunction with the accompanying drawings and specific embodiments.

[0032] Figure 1 The schematic diagram of the yield arm before yielding is as follows: Figure 1 As shown, the scope holding arm of the surgical robot in the specific embodiment of the present invention includes a yielding arm, a parallelogram linkage structure and an end tool (such as an endoscope), the yielding arm includes a rotating joint J1, a lateral movement joint J2 and a rotating joint J3, and the plurality of yielding arms are connected to the turntable through the rotating joint J1. In the present invention, the parallelogram linkage structure and the end tool are in a stationary state relative to the yielding arm during the yielding process, so the end of the yielding arm referred to in the present invention is actually the end tool.

[0033] Figure 2 The flowchart of the method for making way for keeping the terminal position fixed in the present invention is as follows: Figure 2 As shown, the method for giving way to keep the terminal position fixed in the present invention comprises the following steps:

[0034] (1) Establish the kinematic model of the yield arm;

[0035] Determine the DH parameters of the connecting rod structure of each joint of the yielding arm according to the structural design, and establish a kinematic model of the yielding arm relative to a reference coordinate system according to the basic knowledge of robotics; wherein the reference coordinate system can be set as a surgical robot base coordinate system or a turntable base coordinate system;

[0036] (2) constructing a virtual axis based on the unconstrained pose of the end of the yielding arm;

[0037] According to the retraction requirement, determine the constrained pose and unconstrained pose of the end of the retraction arm during the retraction process, and construct a virtual axis with a constrained relationship with the actual joints in the unconstrained pose according to the actual working conditions and the retraction requirement, as Figure 3 shown, specifically:

[0038] (21) According to the retraction requirement, determine the number of degrees of freedom that the end of the retraction arm needs to remain fixed, that is, obtain the constrained pose of the end of the retraction arm during the retraction process;

[0039] In a specific embodiment of the present invention, the spatial pose of the tool at the end of the retraction arm is [p x p y p z δ x δ y δ z , where p x , p y , p z respectively represent the x, y, and z coordinates of the tool at the end of the retraction arm in the reference coordinate system, and δ x , δ y , δ z respectively represent the angles of rotation of the tool at the end of the retraction arm around the x, y, and z axes in the reference coordinate system; in this embodiment, taking the example of keeping the plane position of the tool at the end of the retraction arm fixed during the retraction process, that is, the retraction arm maintains two fixed degrees of freedom during the retraction process, and its constrained pose is [p x p y :

[0040]

[0041] where f x (), f y () respectively represent functions for obtaining the x and y coordinate positions of the corresponding end tool according to the positions of each joint, and θ1, θ2,..., θ n are the positions of each joint of the retraction arm;

[0042] In the present invention, for different retraction requirements, determine the degrees of freedom that the retraction arm needs to remain fixed, and the constrained pose is the pose of the corresponding degrees of freedom that remains fixed;

[0043] (22) Construct a virtual axis for driving the joints corresponding to several degrees of freedom in the unconstrained pose of the end of the retraction arm;

[0044] Obtain the unconstrained pose of the end of the yielding arm in six degrees of freedom in space according to the constrained pose at the end of the yielding arm in step (21). The unconstrained pose is the pose corresponding to the degrees of freedom other than the pose corresponding to the degrees of freedom that are kept fixed, and select the joints corresponding to the degrees of freedom to be driven in its unconstrained pose (that is, select one or more joints from the joints other than the joints corresponding to the degrees of freedom that are kept fixed) to construct the virtual axis θ v , that is:

[0045] θ v = θ0 - kθ k t

[0046] where θ0 is the initial value of the degrees of freedom joint selected in the unconstrained pose; θ k is the rotation angle per unit time, t is the driving time, and k is the driving ratio between the virtual axis and the degrees of freedom joint to be driven, which can be set according to actual needs.

[0047] In the present invention, the number of degrees of freedom of the virtual axis is determined according to the redundant degrees of freedom, that is, the difference between the degrees of freedom of movement of the yielding arm and the number of degrees of freedom that need to be kept fixed at its end; it can be that one virtual axis drives multiple degrees of freedom, or multiple virtual axes drive one degree of freedom.

[0048] In this embodiment, since the constrained pose at the end of the yielding arm is [p x p y , then its unconstrained pose is [δ x δ y δ z p z ; in this embodiment, the number of joints of the yielding arm is three, that is, the joint positions of the yielding arm are θ1, θ2, and θ3 respectively, then the redundant degree of freedom is 1, so the virtual axis θ v can drive the joint movement corresponding to any one of the degrees of freedom δ x , δ y , δ z and p z in the unconstrained pose;

[0049] More specifically, in this embodiment, taking the yielding of the yielding arm rotating uniformly around the y-axis as an example, then the virtual axis θ v drives the joint movement of the degree of freedom δ y , that is, θ v = δ y :

[0050] In this embodiment, it is set that the driving ratio between θ v and the degree of freedom to be driven is 1:1 and drives uniformly in one direction, and the driving displacement is -θ k t, that is:

[0051] δ y=θ0-kθ k t

[0052] (23) Determine the virtual axis θ constructed in step (22) v Whether there is a constraint relationship between the actual joints of the yield arm, the specific constraint relationship is as follows:

[0053] θ v =f v (θ1,θ2,…,θ n )

[0054] Among them, f v () represents the function of obtaining the corresponding posture according to the position of each joint;

[0055] If it does not exist, return to step (22) and reselect the postures corresponding to other degrees of freedom in the unconstrained posture to construct the virtual axis; if it exists, jump to step (24);

[0056] (24) The virtual axis that has a constraint relationship with each actual joint of the yield arm is constructed based on the unconstrained posture of the yield arm end, which can be expressed as follows:

[0057]

[0058] (3) Calculate the virtual axis θ v The actual joint positions of the yielding arm under drive are used to drive the corresponding joint movements of the yielding arm;

[0059] According to the constrained posture of the end of the yielding arm and the joint position driven by the virtual axis, the joint positions of the yielding arm are solved to drive each joint to ensure that the constrained posture of the end of the yielding arm is fixed; Figure 4 As shown, the following steps are included:

[0060] (31) Construct a virtual axis θ v The constraint equations of the joint positions of the yield arm and the constrained posture of the end of the yield arm under driving are:

[0061]

[0062] Among them, [Z1 Z2...Z m ] represents the spatial position of the end of the yield arm [p x p y p z δ x δ y δ z ] is the constrained posture, m is the number of degrees of freedom that the end of the yield arm needs to keep fixed;

[0063] (32) Based on the constraint equations constructed in step (31), the target positions of the joints of the yield arm [θ 1t θ 2t …θ nt ], and control the corresponding joint movement accordingly, such as Figure 5 Shown is a schematic diagram of the yield arm after yielding;

[0064] The present invention further includes the steps of: after driving the corresponding joint of the yielding arm to move, calculating the actual position and posture [Z 1t Z 2t …Z mt ]:

[0065]

[0066] Determine whether the error between the actual posture of the end of the yield arm and its constrained posture exceeds the threshold. If not, send the driver to control the corresponding joint movement; otherwise, stop controlling the joint movement.

[0067] More specifically, the error between the actual pose of the end of the yield arm and its constrained pose is as follows:

[0068]

[0069] Among them, Z 1e , Z 2e , …, Z me are the errors between the corresponding degrees of freedom postures between the actual posture of the end of the yielding arm and its constrained posture, respectively;

[0070] If the errors between the corresponding degrees of freedom of the two positions are within the set threshold, the driver is sent to control the corresponding joint movement; otherwise, the control of the joint movement is stopped.

[0071] In this embodiment, the set thresholds of the errors between the postures of the two corresponding degrees of freedom can be set as follows: the set threshold of the translational degree of freedom is 0.1 mm, and the set threshold of the rotational degree of freedom is 0.05 rad.

[0072] The invention constructs a constrained virtual axis to ensure the uniformity of the virtual axis speed in the process of giving way, thereby avoiding the problem of excessive changes in the speeds of other joints caused by joint driving. Compared with the inverse Jacobian determinant solution method based on posture error, the invention adopts an analytical method or a numerical method, and its accuracy is not affected by the size of the posture error or the speed differential.

[0073] The preferred embodiments of the present invention are described in detail above, but the present invention is not limited to the specific details in the above embodiments. Within the technical concept of the present invention, various equivalent transformations (such as quantity, shape, position, etc.) can be made to the technical scheme of the present invention, and these equivalent transformations all belong to the protection scope of the present invention.

Claims

1. A method for making way while keeping the end pose fixed, characterized in that: Includes steps: Establish the kinematic model of the yield arm; Determine the constrained and unconstrained postures of the end of the yielding arm during the yielding process, and construct a virtual axis for driving the joint corresponding to one or more degrees of freedom in the unconstrained posture; The constructed virtual axis θ v Specifically: θ v = θ 0- kθ k t Among them, θ 0 is the initial value of the degree-of-freedom joint selected in the unconstrained pose; θ k is the rotation angle per unit time, t is the driving time, k is the driving ratio between the virtual axis and the degree-of-freedom joint to be driven; A set of constraint equations for the joint positions of the yield arm and the constrained posture of the end of the yield arm driven by the virtual axis is constructed, and the target positions of the actual joints of the yield arm are obtained by solving the constraint equations, and the corresponding joints of the yield arm are driven to move accordingly.

2. The method for making way while keeping the end pose fixed according to claim 1, characterized in that: According to the degree of freedom that needs to be kept fixed at the end of the yielding arm, the constrained posture and unconstrained posture of the end of the yielding arm during the yielding process are obtained.

3. The method for making way while keeping the end pose fixed according to claim 2, characterized in that: The number of degrees of freedom of the virtual axis is the difference between the degrees of freedom of the movement of the yield arm and the number of degrees of freedom of the end thereof that needs to remain fixed.

4. The method for making way while keeping the end pose fixed according to claim 1, characterized in that: Also includes the steps: After driving the corresponding joint movement of the yielding arm, the actual posture of the end of the yielding arm is calculated, and the error between the actual posture and the constrained posture of the end of the yielding arm is compared, and the control of the corresponding joint movement is stopped when the error between the two exceeds a threshold.

5. The method for making way while keeping the end pose fixed according to claim 4, characterized in that: The error between the actual posture of the end of the yielding arm and the posture of each corresponding degree of freedom of the constrained posture is calculated. If the error between the postures of each corresponding degree of freedom of the two is within a threshold range, a driver is sent to control the corresponding joint movement; otherwise, the control of the joint movement is stopped.

6. The method for making way while keeping the end pose fixed according to claim 5, characterized in that: The specific threshold range is as follows: the translational degree of freedom is set to 0.1 mm , and the rotational degree of freedom is set to 0.05 rad .

7. The method for making way while keeping the end pose fixed according to claim 1, characterized in that: After constructing the virtual axis, determine whether there is a constraint relationship between the virtual axis and the actual joints of the yield arm. If not, reselect the joints corresponding to other degrees of freedom in the non-constrained posture to construct the virtual axis.

Images