A robot telecentric motion control method based on task decoupling

Abstract

The present invention relates to the field of robot motion control, and specifically to a robot telecentric motion control method based on task decoupling. Through robot teaching or pre-input, the calibration and acquisition of remote constraints are realized based on the robot's joint feedback; the definition and representation of the telecentric point on the instrument are realized by defining the insertion depth parameter, and the representation of the instrument telecentric point velocity Jacobian matrix is ​​realized; under the remote point constraint, the control task is decoupled into telecentric constraint control and instrument end position control to achieve stable telecentric control; under the remote space constraint, the telecentric motion control of the instrument is realized by solving the quadratic programming problem; the position control of the instrument end is realized by decomposing the motion of the mechanical arm instrument into the adjustment motion, the insertion motion and the telecentric point motion. The beneficial effect of the present invention is that the end control of the instrument under the remote constraint can be realized by using a 6-degree-of-freedom or redundant mechanical arm, and the 4 degrees of freedom of the instrument end can be controlled.

Description

Technical Field

[0001] The invention relates to the field of robot motion control, in particular to a robot telecentric motion control method based on task decoupling. Background Art

[0002] When the robot is performing tasks, it is often necessary to hold the instrument and operate it with the end of the instrument under the constraints of the environment. In order to prevent the instrument from colliding and interfering with the environmental constraints, special methods are needed to ensure that the instrument rod always passes through the constraint interval. Specifically, a remote constraint is imposed on a certain point (distal point) at the far end of the instrument rod, such as moving or not moving according to a certain rule (point constraint), or only being able to move irregularly within a certain range of space (space constraint). The remote constraint limits the two degrees of freedom of the instrument, so that the instrument rod can only slide axially and adjust its direction with the distal point as the fulcrum.

[0003] The telecentric constraint method is mainly implemented by mechanical means (such as surgical robots) and control methods (such as industrial robots). However, the implementation method of mechanical mechanisms is not flexible and cannot adapt well to dynamic constraints and active telecentric points. The constraints implemented by the current control method are unstable. Therefore, it is necessary to plan a flexible and stable soft telecentric constraint control method. Summary of the invention

[0004] The purpose of the present invention is to provide a method for realizing stable telecentric motion control by means of task decoupling. The end of the instrument is moved to the remote constraint point or the constraint space boundary through the teaching mode, or its position is obtained through pre-input; the telecentric constraint (RCM) is used as the first task of the manipulator, the end position control of the manipulator instrument is used as the second task, and the telecentric motion under the remote point constraint is realized through the null space of the RCM Jacobian; by introducing velocity damping, the control problem is converted into a quadratic programming problem to solve the telecentric motion under the remote space constraint; the motion of the instrument is decomposed into the turning motion, the insertion motion and the telecentric point motion to realize the control of the instrument, avoiding the algorithm singularity in the robot control process.

[0005] The technical solution adopted by the present invention to achieve the above-mentioned purpose is: a robot telecentric motion control method based on task decoupling, comprising the following steps:

[0006] 1) Calibrate and obtain the position of the current remote constraint in the robot's mechanical arm coordinate system and input it into the controller;

[0007] 2) The definition and representation of the distal point on the instrument are realized by defining the insertion depth parameter of the robot's mechanical arm instrument, and the representation form of the Jacobian matrix of the instrument's distal point velocity is realized;

[0008] 3) Constructing the telecentric motion control model of the robotic arm instrument:

[0009] According to the representation of the Jacobian matrix of the telecentric velocity of the instrument, under the remote point constraint, the telecentric motion control model of the manipulator instrument is established by decoupling the control task into telecentric constraint control and instrument end position control.

[0010] According to the representation of the Jacobian matrix of the telecentric velocity of the instrument, under the remote space constraint, the telecentric motion control model of the robotic arm instrument is established by transforming the control task into a quadratic programming problem.

[0011] 4) The position control of the end of the instrument is achieved by decomposing the movement of the robotic arm instrument into adjustment movement, insertion movement and distal point movement.

[0012] The mechanical arm device is fixed on the end joint of the mechanical arm; the mechanical arm device is a rod structure.

[0013] The remote constraints include remote point constraints and remote space constraints; the remote point constraints mean that the remote center point moves according to a set rule or does not move; the remote space constraints mean that the remote center point can only move randomly within a set range of space.

[0014] In step 2), the position of the distal point on the instrument is obtained by defining the insertion depth variable in combination with the forward kinematics of the robot arm, specifically:

[0015] Set the 6 joint angles of the robot arm to q r =[q1,q2,...,q6] T ;

[0016] Set the insertion depth variable λ, then the control variable is set to: q u =[q1,q2,...,q6,λ] T ;

[0017] Set the position of the end of the instrument rod relative to the robot arm coordinate system as p s1 , the position of the instrument rod mounting end relative to the robot arm coordinate system is p s2 , then:

[0018] p s1 =p1+p,p s2 =p2+p

[0019] Wherein, p1 is the position of the end of the instrument rod relative to the end of the robotic arm, p2 is the position of the instrument mounting end relative to the end of the robotic arm, and p is the position of the end of the robotic arm relative to the robotic arm coordinate system;

[0020] The distal point on the instrument shaft is represented by:

[0021] p RCM =p s2+λ(p s1 -p s2 )

[0022] 0≤λ≤1

[0023] Among them, p RCM is the actual position of the apocenter, p s1 is the end position of the instrument, p s2 is the installation end position of the instrument, and λ is the insertion depth variable.

[0024] In the step 2), specifically:

[0025] Far center point p RCM The differential form of is:

[0026]

[0027] Then the Jacobian matrix J of the far center point is RCM for:

[0028]

[0029] Among them, J s1 and J s2 They are p s1 and p s2 The Jacobian matrix, Represents the joint velocity of the robot arm, J s1,v is the Jacobian matrix of the linear velocity at the end position, J s2,v is the Jacobian matrix of the linear velocity at the installation end.

[0030] In step 3), according to the representation of the Jacobian matrix of the telecentric velocity of the instrument, under the remote point constraint, the telecentric motion control model of the manipulator instrument is established by decoupling the control task into telecentric constraint control and instrument end position control, specifically:

[0031] Set the position of the remote constraint to p c , obtained by artificial setting or measurement calibration, and p c Input to the controller; in real-time control, p c Can change dynamically to suit mission requirements;

[0032] For remote point constraints, define the position error e between the remote point and the remote constraint point. RCM for:

[0033] e RCM =p c -p RCM

[0034] Among them, p c is the position of the remote constraint point in the robot coordinate system, pRCM is the position of the apocenter;

[0035] The instrument end position control and telecentric constraint control are controlled separately at the kinematic level, with the telecentric constraint control as the first task and the end position control as the second task. The end position control is mapped through the null space of the Jacobian matrix to avoid interference with the telecentric constraint.

[0036] Then the telecentric motion control model of the robotic arm, that is, the joint velocity of the robotic arm is expressed as:

[0037]

[0038] in, is the Jacobian matrix J RCM The generalized inverse of RCM is the position error between the telecentric point and the remote constraint point, I is the 7×7 unit matrix, is the target robot joint angular velocity, is the speed of the insertion movement.

[0039] In step 3), according to the representation of the Jacobian matrix of the telecentric velocity of the instrument, under the remote space constraint, the telecentric motion control model of the robotic arm instrument is established by transforming the control task into a quadratic programming problem, specifically:

[0040] For remote space constraints, the end position control of the instrument and the telecentric constraint control are controlled separately at the kinematic level. When the telecentric point approaches the boundary of the remote constraint space, velocity damping is applied to the manipulator instrument.

[0041] Then the telecentric motion control model of the robotic arm instrument, that is, the telecentric constraint control, is expressed as:

[0042]

[0043] Among them, ξ is the convergence coefficient, d is the distance between the far center point and the boundary, Control variable speed for the robot arm;

[0044] At the same time, the control of the robotic arm is realized. When the minimum, the quadratic programming algorithm is used to solve the telecentric motion of the instrument, where is the target robot joint angular velocity, is the speed of the insertion movement.

[0045] In step 4), the movement of the robotic arm instrument is decomposed into directional movement, insertion movement and distal point movement, specifically:

[0046] (1) For the directional motion, if the control target is the direction of the end of the robot arm, the pointing error function of the device is defined as:

[0047]

[0048] Among them, r s1,z represents the end position p of the instrument s1 To the installation end position p s2 The target orientation of the rigid segment, f e (q u ) is the error function;

[0049] (2) If the position of the end of the robot arm is taken as the control target, the position error function of the end of the robot arm is defined as:

[0050] f e (q u )=||p t -p s2 ||

[0051] Among them, p t is the target position of the end of the robotic arm instrument;

[0052] By solving the gradient vector of the error function, the joint velocity of the robot arm when achieving pointing control for:

[0053]

[0054] Among them, k e is a positive coefficient, is the gradient of the pointing error;

[0055] (3) For insertion movement, It is expressed as:

[0056]

[0057] Among them, k λ is a positive coefficient, λ d is the target insertion distance, λ is the insertion depth variable;

[0058] (4) For the motion of the far center point, its initial position p c After setting, the controller will give the target motion trajectory, which can be fixed or move according to the controller's instructions;

[0059] (5) The target device end velocity corresponds to the robot arm joint angular velocity and the speed of insertion movement Substitute it into the telecentric motion control model of the robotic arm device to complete the control of the robotic arm device.

[0060] The present invention has the following beneficial effects and advantages:

[0061] 1. The present invention can use a 6-degree-of-freedom or redundant manipulator to achieve end control of the instrument under remote constraints, and can control 4 degrees of freedom at the end of the instrument.

[0062] 2. The remote constraint of the present invention can be either a point constraint or a spatial constraint within a certain range; the remote constraint point or spatial boundary can be set at any position and can change dynamically in real time; high-precision and stable telecentric motion control of the instrument end can be achieved.

[0063] 3. The control method of the present invention has high precision in telecentric motion, high stability and high fluency in controlling the instrument.

[0064] 4. The present invention introduces velocity damping to transform the control problem into a quadratic programming problem in order to solve the telecentric motion under long-range space constraints.

[0065] 5. The movement of the robot arm device of the present invention is decomposed into directional movement, insertion movement and distal point movement to achieve control of the device, avoiding algorithm singularity in the robot control process. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 A schematic diagram of the system of the present invention;

[0067] Figure 2 A schematic diagram of the motion decomposition of the rod body of the mechanical arm instrument of the present invention;

[0068] Figure 3 Flowchart of the control method of the present invention. DETAILED DESCRIPTION

[0069] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention. It should be noted that, for those of ordinary skill in the art, without departing from the concept of the present invention, several variations and improvements can also be made, which all fall within the scope of protection of the present invention.

[0070] like Figure 3 As shown, it is a flow chart of the control method of the present invention, and the present invention specifically includes the following steps:

[0071] 1) Move the robot arm to the remote constraint area through robot teaching, or calibrate and obtain the position of the current remote constraint in the robot arm coordinate system through pre-input;

[0072] In the teaching mode, the controller obtains the joint angle of the current robot arm, obtains the end position of the robot arm according to the joint angle, and obtains the global variable of the remote constraint position according to the end position of the robot arm, and inputs it into the controller;

[0073] 2) The position of the distal point on the instrument is obtained by combining the global variables and the defined insertion depth variables with the forward kinematics of the manipulator;

[0074] Define the insertion depth variable, obtain the expression of the distal point according to the positions of the proximal and distal ends of the instrument rod in the robot arm coordinate system, and obtain the Jacobian matrix of the distal point;

[0075] 3) When the remote constraint is a remote point constraint, the position error between the remote point and the remote constraint point is defined, and the telecentric motion control model of the robotic arm instrument is constructed by decoupling the control task into telecentric constraint control and end position control of the robotic arm instrument;

[0076] When the remote constraint is a remote space constraint, the telecentric motion control model of the robotic arm instrument is constructed by solving the quadratic programming algorithm;

[0077] 4) The control of the robotic arm device is achieved by decomposing the telecentric motion of the robotic arm device into adjustment motion, insertion motion and telecentric point motion.

[0078] like Figure 1 As shown is a system schematic diagram of the present invention, wherein the robot arm and the controller are conventional devices in the field of robotics for controlling robot movement; in this embodiment, the instrument rod body is a rod body structure and is fixedly mounted on the end joint of the robot arm to realize insertion movement, adjustment movement and the combined composite movement.

[0079] The robot arm coordinate system and the position of the far center point of the present invention are defined as follows: Figure 1 As shown, the acquisition of the specific remote constraint position and the definition of the remote center point position are achieved by the following method, specifically:

[0080] The remote constraint position is obtained through teaching mode or pre-input. In teaching mode, move the end of the instrument to the remote constraint point or the remote space constraint boundary point, and the controller obtains the current joint angle of the robot arm, and obtains the end position of the robot arm according to the joint angle, specifically:

[0081] The controller communicates with the robotic arm to obtain the six joint angles of the robotic arm, namely:

[0082] q r =[q1,q2,...,q6] T

[0083] Through the forward kinematics of the robot arm, the end position T1 of the robot arm is obtained, that is:

[0084]

[0085] Where p is the position of the end of the robot arm relative to the robot arm coordinate system. The position of the end of the instrument rod relative to the end of the robot arm is set as p1, the position of the instrument mounting end relative to the end of the robot arm is set as p2, and the position of the remote constraint point or the remote space constraint boundary point in the robot arm coordinate system is expressed as p c ,Right now:

[0086] p c =p1+p

[0087] p c As a global variable; set the global variable p c Input to the controller. In real-time control, p c Can change dynamically to suit mission requirements.

[0088] By combining global variables and defined insertion depth variables with the robot's forward kinematics, the position of the distal point on the instrument is obtained, specifically:

[0089] Set the insertion depth variable λ, then the control variable is set to: q u =[q1,q2,...,q6,λ] T ;

[0090] Set the position of the end of the instrument rod relative to the robot arm coordinate system as p s1 , the position of the instrument rod mounting end relative to the robot arm coordinate system is p s2 , then:

[0091] p s1 =p1+p,p s2 =p2+p

[0092] Where p1 is the position of the end of the instrument rod relative to the end of the robotic arm, p2 is the position of the instrument mounting end relative to the end of the robotic arm, and p is the position of the end of the robotic arm relative to the robotic arm coordinate system.

[0093] The distal point on the instrument shaft is represented by:

[0094] p RCM =p s2 +λ(p s1 -p s2 )

[0095] 0≤λ≤1

[0096] Among them, p RCM is the actual position of the apocenter, p s1 is the end position of the instrument, p s2 is the installation end position of the instrument, and λ is the insertion depth variable.

[0097] From this, the definition of the position of the apocenter can be obtained;

[0098] According to the actual position of the distal point, the insertion depth variable is defined. According to the position of the proximal and distal ends of the instrument rod in the robotic arm coordinate system, the expression of the distal point is obtained, and the Jacobian matrix of the distal point is obtained, which is used as a prerequisite for the orientation movement or insertion movement of the robotic arm instrument, that is:

[0099] Far center point p RCM The differential form of is:

[0100]

[0101] Then the Jacobian matrix J of the far center point is RCM for:

[0102]

[0103] Among them, J s1 and J s2 They are p s1 and p s2 The Jacobian matrix, Represents the joint velocity of the robot arm, J s1,v is the Jacobian matrix of the linear velocity at the end position, J s2,v is the Jacobian matrix of the linear velocity at the installation end.

[0104] like Figure 2 FIG. 1 is a schematic diagram showing the movement of the mechanical arm instrument rod body of the present invention. Figure 2 It can be seen that the control method of the present invention is implemented under a control model based on task decoupling;

[0105] (I) The specific method for obtaining a specific control model for task decoupling is as follows:

[0106] By decoupling the control task into telecentric constraint control and end position control of the robotic arm instrument, a telecentric motion control model of the robotic arm instrument is constructed, specifically:

[0107] For remote point constraints, define the position error e between the remote point and the remote constraint point. RCM for:

[0108] e RCM =p c -p RCM

[0109] Among them, p c is the position of the remote constraint point in the robot coordinate system, p RCM is the position of the apocenter;

[0110] The instrument end position control and telecentric constraint control are controlled separately at the kinematic level, with the telecentric constraint control as the first task and the end position control as the second task. The end position control is mapped through the null space of the Jacobian matrix to avoid interference with the telecentric constraint.

[0111] Then the telecentric motion control model of the robotic arm, that is, the joint velocity of the robotic arm is expressed as:

[0112]

[0113] in, is the Jacobian matrix J RCM The generalized inverse of RCM is the position error between the telecentric point and the remote constraint point, I is the 7×7 unit matrix, is the angular velocity of the robot arm joint corresponding to the target device end velocity, is the speed of the insertion movement.

[0114] For long-range space constraints, that is, when the long-range constraint is a space within a certain range, it means that the telecenter can move within the range space. By moving the end of the instrument to the boundary of the range space for initialization through the teaching mode, its extreme position x in the robot coordinate system can be obtained. lim ,y lim and z lim , and is input into the telecentric motion controller as a global variable. At this time, the insertion depth variable λ is set to 1, and the control variable is set to q u =[q1,q2,...,q6,λ] T .

[0115] When the telecentric coincidence point is close to the boundary, the end position control of the instrument and the telecentric constraint control are controlled separately at the kinematic level, and when the telecentric point is close to the boundary of the telecentric constraint space, velocity damping is applied to the manipulator instrument;

[0116] Then the telecentric motion control model of the robotic arm instrument, that is, the telecentric constraint control, is expressed as:

[0117]

[0118] Among them, ξ is the convergence coefficient, d is the distance between the far center point and the boundary, Control variable speed for the robot arm;

[0119] At the same time, the control of the robotic arm is realized. When the minimum, the quadratic programming algorithm is used to solve the telecentric motion of the instrument, where is the angular velocity of the robot arm joint corresponding to the target device end velocity, is the speed of the insertion movement.

[0120] Therefore, a quadratic programming algorithm can be used to solve the telecentric motion of the instrument.

[0121] (ii) By decomposing the telecentric motion of the robotic arm instrument into the adjustment motion, the insertion motion and the telecentric point motion, and combining the construction of the telecentric motion control model of the robotic arm instrument, the control of the robotic arm instrument is realized:

[0122] (1) For the directional motion, if the control target is the direction of the end of the robot arm, the pointing error function of the device is defined as:

[0123]

[0124] Among them, r s1,z Indicates the end position p of the instrument s1 To the installation end position p s2 The target orientation of the rigid segment, f e (q u ) is the error function;

[0125] (2) If the position of the end of the robot arm is taken as the control target, the position error function of the end of the robot arm is defined as:

[0126] f e (q u )=||p t -p s2 ||

[0127] Among them, p t is the target position of the end of the robotic arm instrument;

[0128] By solving the gradient vector of the error function, the joint velocity of the robot arm when achieving pointing control for:

[0129]

[0130] Among them, k e is a positive coefficient, is the gradient of the pointing error;

[0131] (3) For insertion movement, It is expressed as:

[0132]

[0133] Among them, k λ is a positive coefficient, λ d is the target insertion distance, λ is the insertion depth variable;

[0134] (4) For the motion of the far center point, its initial position p cAfter setting the teaching mode, the controller gives the target motion trajectory, which can be fixed or move according to the controller's instructions.

[0135] (5) The target device end velocity corresponds to the robot arm joint angular velocity and the speed of insertion movement Substitute it into the telecentric motion control model of the robotic arm device to complete the control of the robotic arm device.

[0136] The present invention moves the end of the surgical instrument to the position of the remote constraint point or the boundary of the constraint space through the teaching mode, triggers the setting of the initial telecentric point by a key, or directly sets the telecentric point position by pre-input, and obtains the representation of the telecentric point in the robot coordinate system. The target posture is input, which can be obtained through a controller or an operating handle, and is input into the RCM controller as the target end posture of the instrument. The RCM controller uses the target posture and telecentric point position of the instrument as input to calculate the joint value of the robot arm, and sends it to the robot arm to realize the telecentric motion control of the robot arm under the remote constraint.

[0137] The control method of the present invention realizes flexible soft telecentric constraint. The control method ensures the accuracy of telecentric motion on the one hand, and has strong control stability on the other hand, thus realizing stable and smooth control of the instrument.

[0138] In this specification, the present invention has been described with reference to its specific embodiments. The above embodiments are preferred embodiments of the present invention and are not intended to limit the scope of implementation of the present invention. It should be noted that the present invention is not limited to the above specific implementations. Any improvements, changes, combinations, substitutions, etc. made by those skilled in the art without departing from the principles of the present invention are all within the scope of protection claimed by the claims of the present invention.

Claims

1. A robot telecentric motion control method based on task decoupling, characterized in that: The following steps are involved: 1) Calibrate and obtain the position of the current remote constraint in the robot's mechanical arm coordinate system and input it into the controller; 2) The definition and representation of the distal point on the instrument are realized by defining the insertion depth parameter of the robot's mechanical arm instrument, and the representation form of the Jacobian matrix of the instrument's distal point velocity is realized; 3) Constructing the telecentric motion control model of the robotic arm instrument: According to the representation of the Jacobian matrix of the telecentric velocity of the instrument, under the remote point constraint, the telecentric motion control model of the manipulator instrument is established by decoupling the control task into telecentric constraint control and instrument end position control. According to the representation of the Jacobian matrix of the telecentric velocity of the instrument, under the remote space constraint, the telecentric motion control model of the robotic arm instrument is established by transforming the control task into a quadratic programming problem. 4) The position control of the end of the instrument is achieved by decomposing the movement of the robotic arm instrument into adjustment movement, insertion movement and distal point movement.

2. A robot telecentric motion control method based on task decoupling according to claim 1, characterized in that: The mechanical arm device is fixed on the end joint of the mechanical arm; the mechanical arm device is a rod structure.

3. The robot telecentric motion control method based on task decoupling according to claim 1 is characterized in that: In step 1), the remote constraint includes a remote point constraint and a remote space constraint; the remote point constraint is that the remote center point moves according to a set rule or does not move; the remote space constraint is that the remote center point can only move randomly within a set range of space.

4. The robot telecentric motion control method based on task decoupling according to claim 1 is characterized in that: In step 2), the definition and representation of the distal point on the instrument is achieved by defining the insertion depth parameter of the robot's mechanical arm instrument, specifically: By defining the insertion depth variable and combining the forward kinematics of the robot arm, the position of the distal point on the instrument is obtained, specifically: Set the six joint angles of the robot arm to q r =[q1,q2,...,q6] T ; Set the insertion depth variable λ, then the control variable is set to: q u =[q1,q2,...,q6,λ] T ; Set the position of the end of the instrument rod relative to the robot arm coordinate system as p s1 , the position of the instrument rod mounting end relative to the robot arm coordinate system is p s2 , then: p s1 =p1+p,p s2 =p2+p Wherein, p1 is the position of the end of the instrument rod relative to the end of the robotic arm, p2 is the position of the instrument mounting end relative to the end of the robotic arm, and p is the position of the end of the robotic arm relative to the robotic arm coordinate system; The distal point on the instrument shaft is represented by: p RCM =p s2 +λ(p s1 -p s2 ) 0≤λ≤1 Among them, p RCM is the actual position of the apocenter, p s1 is the end position of the instrument, p s2 is the installation end position of the instrument, and λ is the insertion depth variable.

5. A robot telecentric motion control method based on task decoupling according to claim 4, characterized in that: In the step 2), Far center point p RCM The differential form of is: Then the Jacobian matrix J of the far center point is RCM for: Among them, J s1 and J s2 They are p s1 and p s2 The Jacobian matrix, Represents the joint velocity of the robot arm, J s1,v is the Jacobian matrix of the linear velocity at the end position, J s2,v is the Jacobian matrix of the linear velocity at the installation end.

6. A robot telecentric motion control method based on task decoupling according to claim 5, characterized in that: In step 3), according to the representation of the Jacobian matrix of the telecentric velocity of the instrument, under the remote point constraint, the telecentric motion control model of the manipulator instrument is established by decoupling the control task into telecentric constraint control and instrument end position control, specifically: Set the position of the remote constraint to p c , obtained by artificial setting or measurement calibration, and p c Input to the controller; in real-time control, p c Can change dynamically to suit mission requirements; For remote point constraints, define the position error e between the remote point and the remote constraint point. RCM for: e RCM =p c -p RCM Among them, p c is the position of the remote constraint point in the robot coordinate system, p RCM is the position of the apocenter; The instrument end position control and telecentric constraint control are controlled separately at the kinematic level, with the telecentric constraint control as the first task and the instrument end position control as the second task. The instrument end position control is mapped through the null space of the Jacobian matrix to avoid interference with the telecentric constraint. Then the telecentric motion control model of the robotic arm, that is, the joint velocity of the robotic arm is expressed as: in, is the Jacobian matrix J RCM The generalized inverse of RCM is the position error between the telecentric point and the remote constraint point, I is the 7×7 unit matrix, is the target robot joint angular velocity, is the speed of the insertion movement.

7. A robot telecentric motion control method based on task decoupling according to claim 6, characterized in that: In step 3), according to the representation of the Jacobian matrix of the telecentric velocity of the instrument, under the remote space constraint, the telecentric motion control model of the robotic arm instrument is established by transforming the control task into a quadratic programming problem, specifically: For remote space constraints, the end position control of the instrument and the telecentric constraint control are controlled separately at the kinematic level. When the telecentric point approaches the boundary of the remote constraint space, velocity damping is applied to the manipulator instrument. Then the telecentric motion control model of the robotic arm instrument, that is, the telecentric constraint control, is expressed as: Among them, ξ is the convergence coefficient, d is the distance between the far center point and the boundary, Control the variable speed for the robot arm; At the same time, the control of the robotic arm is realized. When the minimum, the quadratic programming algorithm is used to solve the telecentric motion of the instrument, where is the target robot joint angular velocity, is the speed of the insertion movement.

8. The robot telecentric motion control method based on task decoupling according to claim 7 is characterized in that: In step 4), the movement of the robotic arm instrument is decomposed into directional movement, insertion movement and distal point movement, specifically: (1) For the directional motion, if the control target is the direction of the end of the robot arm, the pointing error function of the device is defined as: Among them, r s1,z represents the end position p of the instrument s1 To the installation end position p s2 The target orientation of the rigid segment, f e (q u ) is the error function; (2) If the position of the end of the robot arm is taken as the control target, the position error function of the end of the robot arm is defined as: f e (q u )=||p t -p s2 || Among them, p t is the target position of the end of the robotic arm instrument; By solving the gradient vector of the error function, the joint velocity of the robot arm when achieving pointing control for: Among them, k e is a positive coefficient, is the gradient of the pointing error; (3) For insertion movement, It is expressed as: Among them, k λ is a positive coefficient, λ d is the target insertion distance, λ is the insertion depth variable; (4) For the motion of the far center point, its initial position p c After setting, the controller will give the target motion trajectory, which can be fixed or move according to the controller's instructions; (5) The target device end velocity corresponds to the robot arm joint angular velocity and the speed of insertion movement Substitute it into the telecentric motion control model of the robotic arm device to complete the control of the robotic arm device.

Images