A method, device and storage medium for real-time control of terminal speed of articulated robot

Abstract

The present invention discloses a method for real-time control of the terminal speed of a joint robot, comprising the following steps: inputting an expected velocity vector of the robot terminal; processing by a first PI controller; decomposing the velocity vector into three components of a spatial coordinate system; inversely transforming the Jacobian matrix into the joint motor speed; processing by a second PI controller; entering a joint drive speed control mode, loading the joint motor speed instruction into a driver speed loop; collecting the real-time speed of the joint; the multi-joint robot executing a preset instruction; filtering by a second filter and synchronously outputting a velocity vector signal to an adder-subtractor; transforming the Jacobian matrix into the terminal speed vector; filtering by a first filter and outputting the real-time velocity vector of the robot terminal, further performing addition and subtraction operations with the expected velocity vector of the terminal joint, thereby adjusting the robot terminal speed in real time. The speed control system of the present invention is more stable and reliable, eliminates error interference, and can control the robot terminal speed vector in real time.

Description

Technical Field

[0001] The present invention relates to the field of medical devices, and in particular to a method, device and storage medium for real-time control of the terminal speed of an articulated robot. Background Art

[0002] In the existing technology, position control technology is used to control the terminal speed of medical joint robots, which are usually planned according to a predetermined path. If the speed and direction of the robot need to be changed midway, the path needs to be re-planned, and the position data needs to be calculated and sent to the servo motors of each joint to change the speed and direction of the robot. In particular, in the terminal joint speed control technology of multi-joint robots, hysteresis and jitter occur, causing unnecessary surgical risks. The existing technical solutions that use position control technology to achieve robot terminal speed control generally have defects such as large computational complexity, slow response, and large speed fluctuations.

[0003] Chinese patent application 2022114994318 proposes a method, system and computer equipment for calculating the joint speed of a robot. The method first establishes an inverse position model, a forward speed model and an inverse speed model of the joint robot; obtains the singular conditional expression of the robot motion, establishes a relationship model between the joint speed and the singular conditional expression, and rewrites the inverse speed model; designs a damping inverse function for each joint; obtains the solution of the joint angle vector through the singular conditional expression, defines the singular area, and determines the singular boundary of the robot; uses the position inverse model to obtain the joint angle vector, optimizes the parameters of the iterative damping inverse function, constructs a new robot speed inverse model, and solves the joint speed of each trajectory point. This technical solution has high requirements on computing power, and the real-time response loudness needs to be further verified and improved.

[0004] Based on the shortcomings of existing technologies, it is necessary to improve the speed control method of the joint ends of medical robots in order to achieve faster response speed, lower computing power requirements, better control effect, and no jitter or fluctuation. Summary of the Invention

[0005] In order to address the above-mentioned deficiencies in the prior art, the present invention provides a method, device and storage medium for real-time control of robot joint speed.

[0006] In a first aspect, the present invention provides a method for real-time control of the terminal velocity of an articulated robot, comprising the following steps:

[0007] A method for real-time control of the terminal speed of an articulated robot comprises the following steps:

[0008] Step 101: Input the expected velocity vector of the end joint robot ;

[0009] Step 102: first adder-subtractor processing;

[0010] Step 103: first PI controller processing;

[0011] Step 104: Decompose the velocity vector into three components Vx, Vy and Vz in the spatial coordinate system;

[0012] Step 105: Inverse transformation of Jacobian matrix to joint motor velocity , further transform the speed vector of each joint motor into Enter the second adder and subtractor;

[0013] Step 106: second adder-subtractor processing;

[0014] Step 107: second PI controller processing;

[0015] Step 108: Enter the joint drive speed control mode and set the joint motor speed instruction Loaded to the drive speed loop;

[0016] Step 109: Collecting joint speeds, collecting the speeds of joint motors, and obtaining real-time feedback of the end joint real-time speeds;

[0017] Step 110: The multi-joint robot executes the preset instructions;

[0018] Step 111: The second filter performs filtering, filters the velocity vector signal, and synchronously outputs the velocity vector signal. To the second adder and step 112;

[0019] Step 112: Convert the Jacobian matrix to the terminal velocity vector;

[0020] Step 113: The first filter performs filtering and outputs the real-time velocity vector of the robot end joint , further transforming the expected velocity vector of the end joint into and the real-time velocity vector of the end joint Perform addition and subtraction operations and repeat the above steps to complete the real-time adjustment of the robot's end joint speed.

[0021] Furthermore, the PI controller is a proportional-integral module.

[0022] Furthermore, the step 105 further calculates the Jacobian inverse matrix of Vx, Vy and Vz according to Formula 1: Calculate the motor velocity vector of each joint, and further transform the motor velocity vector of each joint Enter the second adder-subtractor, where: is the joint motor speed, is the velocity Jacobian matrix, is the velocity vector.

[0023] Furthermore, the step 107 is specifically as follows: data processing is performed according to the output of the adder and subtractor, and a speed instruction is further output. ,in is the joint motor speed command.

[0024] Furthermore, the step 112 is specifically as follows: based on the motor speed of each joint , according to formula 2: Solve the velocity vectors in three directions of the robot end and the space coordinate system, and synthesize the velocity vector ,in is the joint motor speed, is the velocity Jacobian matrix, is the velocity vector.

[0025] In a second aspect, the present invention provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the real-time control method for the terminal velocity of an articulated robot as described in any one of the first aspects.

[0026] In a third aspect, the present invention provides a robot joint speed real-time control device, comprising:

[0027] one or more processors;

[0028] Memory; and

[0029] One or more computer programs, wherein the one or more computer programs are stored in the memory and are configured to be executed by the one or more processors, and when the processor executes the computer program, the steps of the real-time control method of the terminal velocity of the articulated robot as described in any one of the first aspects are implemented.

[0030] The real-time control method of robot joint speed of the present invention adopts speed control technology to control the speed of the robot terminal, which can change the speed size and direction at any time. After the speed of each joint is obtained by calculation based on the terminal speed vector, the speed of the servo motor is directly controlled.

[0031] This technical solution uses two PI controllers to make the speed control system more stable and reliable, eliminate the interference of errors, especially the errors caused by the data acquisition process. After filtering, the signal is more real and effective. The speed control technology used for the joint drive motor can better adapt to the real-time control of the changes in the robot terminal velocity vector and has a faster response speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 : Flowchart of real-time control of terminal velocity of articulated robot.

[0033] Figure 2 : Schematic diagram of the composition of the real-time control device for the terminal speed of the articulated robot. DETAILED DESCRIPTION

[0034] In order to make the technical problems, technical solutions and beneficial effects to be solved by the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0035] Please refer to Figure 1 The real-time control flow chart of the end velocity of the joint robot includes the following steps:

[0036] Step 101: Input the expected velocity vector of the end joint robot ;

[0037] Step 102: first adder-subtractor processing;

[0038] Step 103: first PI controller processing, wherein the PI controller is a proportional-integral module;

[0039] Step 104: Decompose the velocity vector V into three components of the spatial coordinate system, wherein the velocity vector is decomposed into three components Vx, Vy and Vz of the spatial coordinate system;

[0040] Step 105: The Jacobian matrix is ​​inversely transformed to the joint motor speed by calculating the Jacobian inverse matrix of Vx, Vy and Vz according to Formula 1: Calculate the motor velocity vector of each joint , further transform the speed vector of each joint motor into Enter the second adder and subtractor;

[0041] Step 106: second adder-subtractor processing;

[0042] Step 107: The second PI controller processes the data according to the output of the second adder-subtractor and further outputs the speed instruction. ,in is the joint motor speed instruction;

[0043] Step 108: Enter the joint drive speed control mode and set the joint motor speed instruction Loaded to the drive speed loop;

[0044] Step 109: Collect joint speed, collect the speed of the joint motor, and obtain the real-time feedback of the terminal real-time speed;

[0045] Step 110: The multi-joint robot executes the preset instructions;

[0046] Step 111: The second filter performs filtering, filters the velocity vector signal, and synchronously outputs the velocity vector signal. To the second adder and step 112;

[0047] Step 112: Convert the Jacobian matrix to the terminal velocity vector based on the motor speed of each joint Through formula 2: Solve the velocity vectors in three directions of the robot end and the space coordinate system, and synthesize the velocity vector ,in is the joint motor speed, is the velocity Jacobian matrix, is the velocity vector;

[0048] Step 113: The first filter performs filtering and outputs the real-time velocity vector of the end of the joint robot , further transform the terminal expected velocity vector and the terminal real-time velocity vector Perform addition and subtraction operations and repeat the above steps to complete the real-time adjustment of the end speed of the joint robot.

[0049] The technical solution of the present invention processes the difference between the expected velocity vector of the end of the joint robot and the real-time velocity vector of the end through the first PI controller to obtain the velocity vector V, decomposes the velocity vector of the end of the joint robot into three direction vectors Vx, Vy and Vz in the Cartesian space coordinate system of the robot, and calculates the Jacobian inverse matrix of the robot position at the current position. , the joint motor speed is obtained by transformation , further adopt the second PI controller for smooth transition, and obtain the current speed of each joint motor as feedback , the speed command is obtained through the second PI controller , which is further loaded into the drive speed loop to perform real-time speed control of the robot joints.

[0050] Please refer to Figure 2 Schematic diagram of the composition of the real-time control device for the terminal speed of an articulated robot. The real-time control device 10 for the terminal speed of an articulated robot of the present invention further includes one or more memories 20 and one or more processors 30, wherein the one or more computer programs are stored in the memories 20 and are configured to be executed by the one or more processors 30. When the processor 30 executes the computer program, the steps of the real-time control method for the robot joint speed are implemented.

[0051] The present invention also provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps described in the real-time control method for robot joint speed are implemented.

[0052] The real-time control scheme of the terminal speed of the joint robot of the present invention adopts two PI controllers, making the speed control system more stable and reliable, eliminating error interference, especially eliminating the error caused by the data acquisition process. After the signal is filtered by the first filter and the second filter, the signal is more real and effective. The speed control technology is adopted for the joint drive motor, which can be more adaptable to the real-time control of the change of the robot terminal speed vector and has a faster response speed.

[0053] Those skilled in the art will appreciate that all or part of the steps in the various methods of the above embodiments can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium, which may include: a read-only memory (ROM), a random access memory (RAM), a disk or an optical disk, etc.

[0054] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A real-time control method for the terminal speed of an articulated robot, characterized in that: The following steps are involved: Step 101: Input the expected velocity vector of the end joint of the articulated robot ; Step 102: Convert the expected velocity vector of the end joint Processed by the first adder and subtractor; Step 103: Processing the data output by the first adder-subtractor through the first PI controller to obtain a velocity vector V; Step 104: Decompose the velocity vector V into three components Vx, Vy and Vz in the spatial coordinate system; Step 105: The three components Vx, Vy and Vz are inversely transformed into the joint motor velocity vector through the Jacobian matrix , further transform the speed vector of each joint motor into Enter the second adder and subtractor; Step 106: Convert Joint Motor Velocity Vectors Processed by the second adder-subtractor; Step 107: The second PI controller processes the data according to the output of the second adder and subtractor, and further outputs the speed instruction. ,in is the joint motor speed instruction; Step 108: Enter the joint drive speed control mode and set the joint motor speed instruction Loaded to the drive speed loop; Step 109: Collecting joint speeds, collecting the speeds of joint motors, and obtaining real-time feedback of the end joint real-time speeds; Step 110: The multi-joint robot follows the joint motor speed instructions Execute preset instructions; Step 111: The second filter performs filtering to filter the real-time velocity of the end joint and synchronously outputs the velocity vector signal To the second adder and step 112; Step 112: Jacobian matrix transformation to terminal velocity vector ; Step 113: The first filter performs filtering and outputs the real-time velocity vector of the robot end joint , further transforming the expected velocity vector of the end joint into and the real-time velocity vector of the end joint The first adder and subtractor performs addition and subtraction operations, and the above steps are repeated to complete the real-time adjustment of the robot end joint speed.

2. The method for real-time control of the terminal velocity of an articulated robot according to claim 1, wherein: The PI controller is a proportional-integral module.

3. The method for real-time control of the terminal velocity of an articulated robot according to claim 1, wherein: The step 105 further calculates the Jacobian inverse matrix of Vx, Vy and Vz according to formula 1: Calculate the motor velocity vector of each joint, and further transform the motor velocity vector of each joint Enter the second adder-subtractor, where: is the velocity Jacobian matrix.

4. The method for real-time control of the terminal velocity of an articulated robot according to claim 1, wherein: The step 112 is specifically as follows: based on the velocity vector signal , according to formula 2: Solve the velocity vectors of the robot end in three directions in the spatial coordinate system and synthesize the terminal velocity vector .

5. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method for real-time control of the terminal velocity of an articulated robot according to any one of claims 1 to 4 are implemented.

6. A real-time control device for robot joint speed, comprising: one or more processors; Memory; as well as One or more computer programs, wherein the one or more computer programs are stored in the memory and are configured to be executed by the one or more processors, characterized in that when the processor executes the computer program, the steps of the real-time control method of the terminal speed of the articulated robot according to any one of claims 1 to 4 are implemented.

Images