A method for maintaining the pose of the end effector

Abstract

The present invention discloses a method for maintaining the pose of an end effector, comprising: establishing a kinematic model of the end effector and determining a constraint equation; calculating the constrained pose of the end effector according to the positions of the joints of the end effector; updating the constraint equation after the movement of the moving joints in the end effector, calculating the target positions of the other joints in combination with the constrained pose of the end effector, and driving the corresponding joints to move. By determining the constrained or fixed pose and establishing a constraint equation containing the position variables of the joints of the end effector, when one or more of the joints move, the target positions of the other joints are solved by using an analytical method or a numerical method; at the same time, by calculating the actual pose after the joint movement in real time, it is judged whether the deviation from the constrained pose is too large, so as to ensure the safety in the process of maintaining the pose of the end effector.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and particularly to a method for maintaining the pose of an end effector. Background Art

[0002] More and more devices are being replaced by electronic devices. For example, medical robots are applied in the medical treatment or auxiliary medical treatment in hospitals and clinics to achieve semi-autonomous or fully autonomous operations, and they can complete service work beneficial to human health.

[0003] In the fields of human-robot collaboration, surgical robots, etc., the end effector is generally used for operations such as positioning, grasping, and transporting, and redundant joints are usually designed to ensure the flexibility of the end effector operation. When redundant joints exist, multiple different combinations of joint positions can be obtained to ensure that the end effector maintains a fixed pose. This is the null space concept, that is, the technology of ensuring a fixed end pose during joint movement. In actual application processes, the position and pose to be maintained can be selected.

[0004] In the prior art, first, the difference between the actual transformed pose and the reference transformed pose of the end effector is determined, and the mapping relative error of each joint is solved by using the inverse Jacobian determinant to compensate for the movement of the joints to ensure the fixed pose of the end effector. In some examples, compensating for joint errors is also referred to as joint velocity. When the pose error is large, the joint velocity error solved in the same differential interval will increase, and the accuracy cannot be guaranteed. Moreover, the greater the error, the worse the compensation accuracy. At the same time, the change of the tool pose due to the error may lead to other undesirable consequences such as injury to personnel adjacent to the tool and damage to equipment adjacent to the tool. Summary of the Invention

[0005] Object of the Invention: Aiming at the above deficiencies, the present invention proposes a method for maintaining the pose of an end effector, the accuracy of which is not affected by the magnitude of the pose error, and at the same time, damage to personnel or equipment can be avoided.

[0006] Technical Solution:

[0007] A method for maintaining the pose of an end effector includes:

[0008] Establish a kinematic model of the end effector and determine the constraint equation;

[0009] Calculate the constrained pose of the end effector according to the positions of the joints of the end effector;

[0010] Update the constraint equation after the movement of the moving joints in the end effector, and calculate the target positions of the other joints in combination with the constrained pose of the end effector, and drive the corresponding joints to move.

[0011] It further includes the steps:

[0012] Calculate the actual pose of the end effector based on the positions of the moving joints and other joints.

[0013] Calculate the deviation between the actual pose of the end effector and the constrained pose, and stop the drive control of each joint when the deviation exceeds the set threshold range.

[0014] Determine the constraint equation according to the degrees of freedom that the end effector needs to remain fixed.

[0015] Both the moving joints and other joints include several joints, and one or more joints in the moving joints move due to trajectory control or external force control.

[0016] Advantageous effects:

[0017] 1. By determining the constrained or fixed pose, the present invention establishes a constraint equation including the position variables of each joint of the end effector. When one or more joints move, the target positions of other joints are solved by the analytical method or the numerical method.

[0018] 2. The present invention calculates the actual pose after joint movement in real time, judges whether the deviation from the constrained pose is too large, and stops in time when the pose error is too large, so as to ensure the safety during the process of maintaining the pose of the end effector. Description of the drawings

[0019] Figure 1 Schematic diagram of the surgical robot according to the example of the present invention;

[0020] Figure 2 Flowchart of the method for maintaining the tool position according to the present invention.

[0021] Among them, 1 is a surgical robot, 11 is a turntable, 12 is a robotic arm, and 13 is an instrument. Detailed implementation manners

[0022] The present invention will be further clarified below with reference to the drawings and specific embodiments.

[0023] As Figure 1 shown, the surgical robot 1 is provided with a turntable 11, and a number of robotic arms 12 are arranged on the turntable 11. An instrument 13 is arranged at the end of the robotic arm 12. The robotic arm 12 can rotate with the turntable 11 and drive the movement of the end instrument 13. At the same time, each instrument 13 itself has several degrees of freedom of movement, and the pose of each instrument 13 can be adjusted by controlling the positions of the degrees of freedom joints of the robotic arm 12 and the degrees of freedom of movement of each instrument 13 itself.

[0024] During the process of robot-assisted surgery, it is usually necessary to adjust the system components to a specific position to obtain a larger surgical workspace while ensuring that the pose of the end effector is fixed. For example, it is necessary to align with a reference target, that is, the rotation center of the turntable is aligned with the reference point of the end effector whose pose is fixed. Usually, during the alignment process, it is necessary to keep the pose of the end effector fixed.

[0025] In the present invention, the joints of the end effector form a structure with redundant characteristics. Then, the pose of the end effector is determined by the positions of the joints of the end effector. In practical applications, to determine the constrained or fixed pose, a constraint equation including the positions of the joints of the end effector is established. When the moving joints move, the compensation amounts of other joints are solved through the constraint equation. At the same time, the actual pose after compensation is calculated, and it is judged whether the deviation from the fixed pose is too large, so as to ensure the safety of the tool during the movement process.

[0026] The method for maintaining the pose of the end effector in the present invention is as Figure 2 shown. In the embodiments of the present invention, the end effector is defined as having two joints. The first joint, i.e., the moving joint, is the joints of the turntable, which can move due to trajectory control or external force control; the second joint is defined as the joints of the end effector, which needs to have a certain compensation amount after the first joint moves to ensure that the pose of the end effector remains fixed. Of course, in the present invention, the number of joints of the end effector is not fixed and can be determined according to actual needs. This embodiment only provides a possibility.

[0027] It includes the following steps:

[0028] (1) Establish a kinematic model of the end effector;

[0029] According to the DH parameters of the link structure of each joint of the end effector, and based on the basic knowledge of robotics, a kinematic model T of the end effector relative to the reference coordinate system is established, which can be expressed as:

[0030]

[0031] where, n x 、n y 、n z refer to the orientation of the x-axis of the coordinate system where the kinematic model of the end effector is located in the reference coordinate system; o x 、o y 、o z refer to the orientation of the y-axis of the coordinate system where the kinematic model of the end effector is located in the reference coordinate system; a x 、a y 、a z refer to the orientation of the z-axis of the coordinate system where the kinematic model of the end effector is located in the reference coordinate system; p x 、py , p z refers to the coordinates of the origin of the kinematic model of the end effector in the reference coordinate system, that is, the coordinates of the end effector in the reference coordinate system;

[0032] (2) Determine the constraint equations;

[0033] According to the actual application scenario, determine the degrees of freedom that the tool needs to remain fixed (up to six degrees of freedom, six degrees of freedom are taken as an example in the present invention), that is, obtain the constraint equations including the positions of the joints of the tool;

[0034] Keep the spatial pose of the end effector [p x p y p z q x q y q z fixed, q z , q y , q z refers to the angles of rotation of the end effector around the x, y, and z axes respectively in the reference coordinate system; Solve the constraint equations including the first joint position variable and the second joint position variable , where u represents the number of joints included in the first joint, represents the position of the i-th joint in the first joint; v represents the number of joints included in the second joint, represents the position of the j-th joint in the second joint; then the constraint equation can be expressed as:

[0035]

[0036] where f x (), f y (), f z () respectively represent the functions of obtaining the corresponding coordinate positions according to the joint positions, g x (), g y (), g z () respectively represent the functions of obtaining the rotation angles around the corresponding coordinate axes according to the joint positions;

[0037] (3) Calculate the constrained pose of the end effector;

[0038] Obtain the constrained positions φ 10 and φ 20 of the first joint and the second joint, and solve the constrained pose [p x0 p y0 p z0 q x0 qy0 q z0 As the fixed pose, it can be expressed as:

[0039]

[0040] (4) Update the constraint equation after the first joint movement;

[0041] When the first joint moves due to trajectory control or external force control, the actual position φ of the first joint is obtained in real time 1n , and substituting it into the constraint equation in step (2), it can be expressed as:

[0042]

[0043] (5) Solve for the target position of the second joint;

[0044] Combining the constrained pose obtained in step (3) and the updated constraint equation obtained in step (4) to solve for the target position φ of the second joint 2n , which can be expressed as:

[0045]

[0046] (6) Drive the second joint to move: Control the second joint to move according to the target position of the second joint obtained in step (5).

[0047] Furthermore, after driving the second joint to move, the following steps are also included:

[0048] (61) Calculate the actual pose of the end effector;

[0049] According to the position φ of the first joint obtained in step (4) 1n and the target position φ of the second joint obtained in step (5) 2n Calculate the actual pose [p xn p yn p zn q xn q yn q zn ;

[0050]

[0051] (7) Calculate the fixed pose [p x0 p y0 p z0 q x0 q y0 q z0 and the actual pose [p xn p yn pzn q xn q yn q zn Deviation between

[0052]

[0053] If any of the deviations exceeds the threshold, stop the drive control of the second joint movement; if none of them exceed the set threshold, return to step (3); in the present invention, p xe and p ye and p ze are set with a threshold of 0.1 mm, and q xe and q ye and q ze are set with a threshold of 0.05 rad.

[0054] The present invention determines the constrained or fixed pose, establishes a constraint equation including the position variables of each joint of the end effector. When one or more joints move, the analytical method or numerical method is used to solve the target positions of other joints. At the same time, the actual pose after joint movement is calculated, and it is judged whether the deviation from the constrained pose is too large, so as to ensure the safety during the process of maintaining the pose of the end effector.

[0055] The preferred embodiments of the present invention have been 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 solutions of the present invention, and these equivalent transformations all belong to the protection scope of the present invention.

Claims

1. A method for maintaining the pose of the end effector, characterized in that: Including: Step (1) Establish the kinematic model T of the end effector: where n x , n y , n z refers to the orientation of the x-axis of the coordinate system where the kinematic model of the end effector is located in the reference coordinate system; o x , o y , o z refers to the orientation of the y-axis of the coordinate system where the kinematic model of the end effector is located in the reference coordinate system; a x , a y , a z refers to the orientation of the z-axis of the coordinate system where the kinematic model of the end effector is located in the reference coordinate system; p x , p y , p z refers to the coordinates of the origin of the coordinate system where the kinematic model of the end effector is located in the reference coordinate system, that is, the coordinates of the end effector in the reference coordinate system; Step (2) Determine the constraint equation: Taking the moving joint as the first joint, which can move due to trajectory control or external force control; taking each joint of the end effector as the second joint, which needs to have a certain compensation amount after the first joint moves to ensure that the pose of the end effector remains fixed; Maintain the spatial pose of the end effector [p x p y p z q x q y q z fixed, q z and q y and q z refer to the angles by which the end effector rotates about the x, y, and z axes respectively in the reference coordinate system; Solve the constraint equations involving the first joint position variable and the second joint position variable where u represents the number of joints included in the first joint, represents the position of the i-th joint in the first joint; v represents the number of joints included in the second joint, represents the position of the j-th joint in the second joint; Then the constraint equation is expressed as: Among them, f x (), f y (), f z () respectively represent functions for obtaining corresponding coordinate positions according to the positions of each joint, and g x (), g y (), g z () respectively represent functions for obtaining the rotation angles around the corresponding coordinate axes according to the positions of each joint; Step (3) obtains the constrained positions φ of the first joint and the second joint 10 and φ 20 , and solves for the constrained pose [p x0 p y0 p z0 q x0 q y0 q z0 of the end effector by solving the kinematic model of the end effector established in step (1), which is expressed as: Step (4) obtains the actual position φ of the first joint in real time 1n , and updates the constraint equation after the movement of the first joint: Step (5) calculates the target position φ of the second joint by combining the constrained pose of the end effector obtained in step (3) and the updated constraint equation obtained in step (4). 2n : Step (6) Drive the second joint to move according to the target position of the second joint obtained in step (5).

2. The method for maintaining the pose of the end effector according to claim 1, wherein: It also includes step (7): The first joint φ obtained according to step (4) 1n and the target position φ of the second joint obtained in step (5) 2n Calculate the actual pose [p xn p yn p zn q xn q yn q zn of the end effector Calculate the actual pose [p xn p yn p zn q xn q yn q zn and the deviation from the constrained pose [p x0 p y0 p z0 q x0 q y0 q z0 : If any one of the deviations exceeds the threshold, stop the drive control of the second joint movement; if none of them exceed the set threshold, return to step (3).

Images