WO2023000946A1

WO2023000946A1 - Control unit for robot system, robot system, and control method for robot system

Info

Publication number: WO2023000946A1
Application number: PCT/CN2022/102771
Authority: WO
Inventors: 贺岩
Original assignee: 上海捷勃特机器人有限公司
Priority date: 2021-07-19
Filing date: 2022-06-30
Publication date: 2023-01-26
Also published as: CN113263503B; CN113263503A

Abstract

A control unit (300), used for outputting a current instruction to a driving unit (400) of each joint in a robot system according to an instruction. The control unit (300) comprises: a motion planning module (310) generating a point position instruction according to a motion instruction for planning a motion trajectory of the robot system; an overall acceleration control loop module (320) used for generating a first force instruction for each joint on the basis of the point position instruction and acceleration information of any point in the robot system; a single-joint control loop module (330) used for generating a second force instruction for each joint on the basis of the point position instruction and position information of each joint in the robot system; a force instruction fusion module (340) used for calculating and generating a force fusion instruction for each joint on the basis of the first force instruction and the second force instruction; and a current control loop module (350) used for generating a current instruction for driving the driving unit (400) according to the force fusion instruction.

Description

A control unit for a robot system, a robot system, and a control method for the robot system

technical field

The invention belongs to the field of industrial robots, and in particular relates to a control unit of a robot system, a robot system and a control method of the robot system.

Background technique

Industrial robots are multi-joint manipulators or multi-degree-of-freedom machine devices widely used in the industrial field. They have certain automation and can realize various industrial processing and manufacturing functions by relying on their own power energy and control capabilities. They are widely used in electronics, logistics, etc. , chemical industry and other industrial fields.

Existing industrial robot systems, such as the typical 6-joint series collaborative robot in the industry, usually have a certain degree of flexibility between the connecting rod of the mechanical arm and each joint due to considerations of operability and safety, and the overall weight Lighter, which leads to the possibility of shaking of the mechanical arm in the practical application of industrial robots. When the overall movement speed is high, the mechanical arm will shake for a long time after moving to the target position, resulting in a decrease in the accuracy of pick and place. The working tempo is slow.

In addition, existing industrial robots mainly judge whether the robot arm has collided with the current feedback of the joint motor in the robot arm. This method is not sensitive enough, and it is prone to misjudgment and potential safety hazards.

In the existing technical solutions in the industry, by setting displacement, speed, and force sensors at each joint of the robot arm, each joint is precisely controlled through the feedback of multiple sensors, and whether a collision occurs during operation is judged. Introducing a feedback loop formed by multiple sensors can improve the technical defects caused by the motor current feedback alone, but on the other hand, it will greatly increase the amount of calculations in the control loop of the robot system. In addition, the cost of the existing force sensor is relatively high. Adding force sensors at each joint of an articulated manipulator increases the cost of the overall robotic system.

Contents of the invention

Due to the above-mentioned technical problems in the actual application of the robot control system of the prior art solution, the object of the present invention is to provide a control unit, a robot system and a control method of the robot system to solve the above-mentioned technical problems existing in the prior art solution , while improving the end shake of the mechanical arm and realizing sensitive collision detection, it avoids the increase of the calculation amount of the control loop, enhances the robustness of the control system, and improves the working efficiency of the system under the condition of effectively controlling the cost of the robot system.

The technical scheme of the robot system provided by the present invention specifically includes:

A control unit, used in a robot system including multiple joints, outputs current instructions to the drive units of each joint in the robot system according to instructions, and the control unit includes:

A motion planning module generates a point instruction according to the instruction, which is used for the motion trajectory planning of the robot system;

The overall acceleration control loop module is used to generate the first force command of each joint based on the point command and the acceleration information of any point in the robot system;

A single-joint control loop module, configured to generate a second force command for each joint based on the point position command and the position information of each joint of the robot system;

A force command fusion module, configured to calculate and generate a fusion force command for each joint based on the first force command and the second force command;

A current control loop module, configured to generate a current command to drive the driving unit according to the fusion force command.

Preferably, the single-joint control loop module includes: a kinematics inverse solution module, used to generate the joint target position of each joint based on the point instruction; a position control loop module, used to generate the joint target position based on the point instruction, the The joint target position is used to generate a joint speed command; the speed control loop module is used to generate a second force command based on the joint speed command and the joint position information.

Preferably, the control unit further includes: a collision judging module, used to judge whether the robot system collides according to the point command and the acceleration information, and when it is judged that a collision occurs, send a message to the force command fusion module sending a signal to modify the fusion power command; otherwise, not to modify the fusion power command.

Preferably, the control unit further includes a preset threshold. When it is judged that a collision occurs, the collision judgment module calculates the acceleration deviation value of each joint based on the point command and the acceleration information, if any dimension value is greater than When it is equal to the preset threshold, send a stop operation command to the force command fusion module, set the fusion force command to 0, and stop the drive unit; otherwise, send a force command to the force command fusion module Hold the instruction, set the fusion force instruction as the fusion force instruction of the previous cycle, and make the drive unit enter the force holding state.

Preferably, the control unit further includes a storage module for storing preset parameters.

Preferably, the control unit further includes a communication preprocessing module for preprocessing the external information received by the control unit; the communication preprocessing module includes an interface register for storing preprocessed data.

Preferably, the control unit is integrated into one chip.

Preferably, the chip is integrated with a dual-core or multi-core processor and an FPGA processor, and when the control unit is running, the motion planning module runs on a core of the dual-core or multi-core processor; the overall acceleration control loop module, The single-joint control loop module and the force instruction fusion module run on another core of the dual-core or multi-core processor; the current control loop module runs on an FPGA processor.

The present invention also provides a robot system, comprising:

The robot body includes a multi-joint mechanical arm, and a plurality of drive units, the number of the drive units corresponds to the number of joints of the multi-joint mechanical arm, and they are respectively arranged on each joint of the multi-joint mechanical arm for Drive each joint to turn or move;

A plurality of position sensors, the number of the position sensors corresponds to the number of joints of the multi-joint manipulator, which are respectively arranged at each joint of the multi-joint manipulator, and are used to detect the joint position information of each joint;

An inertial sensing unit, the inertial sensing unit is arranged on the multi-joint mechanical arm, and is used to detect the acceleration information of the installation position of the inertial sensing unit;

The robotic system includes the control unit of any one of the preceding items.

Preferably, the point instruction includes an initial joint position instruction, an initial joint velocity instruction, and a position instruction, a velocity instruction, and an acceleration instruction in Cartesian space of each joint in the robot system.

Preferably, the overall acceleration control loop module calculates the overall force deviation value of the multi-joint manipulator and the joint force deviation value of each joint according to the difference between the acceleration command in the Cartesian space and the acceleration information , the first force command of each joint is calculated and generated based on the joint force deviation value and the force command of the previous cycle.

Preferably, the position sensor is a motor encoder.

In addition, the present invention also provides a control method of the robot system,

The robotic system includes:

The robot body includes a multi-joint mechanical arm and multiple drive units for driving each joint to turn or move;

A plurality of position sensors are used to detect the joint position information of each joint;

An inertial sensing unit, the inertial sensing unit is arranged at the front end of the multi-joint mechanical arm, and is used to detect the acceleration information of the inertial sensing unit at the setting position;

a control unit, configured to output a current command to the drive unit of each joint in the multi-joint robot system according to the command,

The control methods include:

In the motion trajectory planning step, a point command is generated according to the command;

Running the overall acceleration control loop step, generating a first force command for each joint based on the point command and the acceleration information of any point in the robot system;

Running the single joint control loop step, generating a second force command for each joint based on the point command and the position information of each joint of the robot system;

The force command fusion step is to calculate and generate fusion force commands for each joint based on the first force command and the second force command;

Running a current control loop step to generate a current command for driving the drive unit based on the fusion force command.

Preferably, the point commands include initial joint position commands, initial joint speed commands, and position commands, speed commands, and acceleration commands in Cartesian space for each joint in the robot system.

Preferably, the step of running the overall acceleration control loop includes: calculating the overall force deviation value of the multi-joint manipulator and the joint force of each joint according to the difference between the acceleration command in the Cartesian space and the acceleration information The deviation value, the first force command of each joint is calculated and generated based on the joint force deviation value and the force command of the previous cycle.

Preferably, the step of running the single-joint control loop includes: a kinematics inverse solution step, based on the point command, generating the joint target position of each joint; running a position control loop step, based on the point command, the joint The target position is to generate a joint speed command; the step of running the speed control loop is to generate a second force command based on the joint speed command and the joint position information.

Preferably, the control method further includes: a collision judging step, judging whether the robot system collides according to the point command and the acceleration information, and if judging that a collision occurs, sending a signal to the force command fusion module, The fusion force command is corrected, and if no collision is determined, the fusion force command is not corrected.

Preferably, the collision judging step further includes, based on the acceleration information in the Cartesian space, calculating the acceleration information of each joint, and judging whether the directions of the point command and the acceleration information of each joint are the same, when any When the accelerations in the one-dimensional direction are opposite, it is judged that there is a collision, otherwise, it is judged that there is no collision.

Preferably, the collision judging step includes: calculating the acceleration deviation value based on the point command and the acceleration information of each joint, and if any of the dimension values is greater than or equal to a preset threshold value, sending a stop operation to the force command fusion module command, set the fusion force command to 0, so that the drive unit stops running; otherwise, send a force maintenance command to the force command fusion module, and set the fusion force command as the fusion force command of the previous cycle , so that the drive unit enters the force holding state.

By applying a robot system and a control method of the robot system proposed by the present invention, the problems existing in the prior art can be solved from the source, and the following advantages are brought:

First, the control unit, the robot system and the control method of the robot system provided by the present invention realize the control of the multi-joint manipulator through the comprehensive application of the overall acceleration control loop module, single joint control loop The control can improve the end shake phenomenon of the industrial robot during the operation process and improve the working efficiency of the robot system without increasing the cost of the robot system;

Second, the control unit, the robot system and the control method of the robot system provided by the present invention can sensitively detect the collision of any joint in the multi-joint manipulator during operation through the acceleration deviation and direction judgment, so as to avoid misjudgment , and after a collision is detected, enter the force holding state or stop operation state in time, thereby reducing the risk of accidents;

Third, the machine control unit, the robot system, and the control method of the robot system provided by the present invention do not need to perform any matrix inversion operation in each control loop, and the calculation amount of the control loop is small, which is conducive to improving the robot system. Overall real-time;

Fourth, the control unit, the robot system and the control method of the robot system provided by the present invention detect and compare the acceleration of the front end of the multi-joint manipulator, and decompose it into the control loop of each joint, without calculating the acceleration by differential velocity information Information, to avoid further amplification of the noise contained in the detected speed information in the differential process, thereby enhancing the robustness of the robot system.

These and other objects and advantages will become more apparent to those skilled in the art after reading the following portions of this specification in conjunction with the accompanying drawings.

Description of drawings

The above summary of the invention and the following specific implementation methods of the present invention will be better understood when read in conjunction with the accompanying drawings. It should be noted that the drawings are merely examples of the claimed invention. In the drawings, the same reference numerals represent the same or similar elements.

FIG. 1 is a block diagram of the robot system of the present invention.

FIG. 2 is a schematic flowchart of the control method of the robot system of the present invention.

The reference signs are as follows:

100. Position sensor

200. Inertial sensing unit

300, control unit

310. Motion Planning Module

320. Overall acceleration control loop module

330. Single joint control loop module

331. Kinematics inverse solution module

332. Position control loop

333. Speed control loop

340. Force Command Fusion Module

350. Current control loop module

360, collision judgment module

370: storage

380. Communication preprocessing module

400, drive unit

detailed description

The detailed features and advantages of the present invention are described in detail below in the specific embodiments, the content of which is sufficient to enable any person skilled in the art to understand the technical content of the present invention and implement it accordingly, and according to the specification, claims and drawings disclosed in this specification , those skilled in the art can easily understand the related objects and advantages of the present invention.

1 to 2 show a preferred embodiment of the control unit, the robot system and the control method of the robot system provided by the present invention.

Wherein, Fig. 1 shows a schematic diagram of modules of the robot system of the present invention.

A robotic system comprising:

The robot body includes a multi-joint mechanical arm (not shown in the figure), and a plurality of drive units 400, the number of which corresponds to the number of joints of the multi-joint mechanical arm, which are respectively arranged on each joint of the multi-joint mechanical arm for driving Each joint turns or moves;

A plurality of position sensors 100, the number of which corresponds to the number of joints of the multi-joint manipulator, are respectively arranged at each joint of the multi-joint manipulator for detecting the joint position information of each joint;

An inertial sensing unit 200, arranged at the front end of the multi-joint manipulator, for detecting the acceleration information of the position where the inertial sensing unit is set;

A control unit 300, configured to output current commands to the drive units of each joint in the robot system according to the commands, including:

The motion planning module 310 generates point and position commands according to the commands, which are used for motion trajectory planning of the robot system. In one embodiment, the point instruction includes an initial joint position instruction, an initial joint velocity instruction, and a position instruction, a velocity instruction, and an acceleration instruction in Cartesian space of each joint in the robot system.

The overall acceleration control loop module 320 is used to generate the first force command of each joint based on the point command and the acceleration information of any point in the robot system. In one embodiment, the overall acceleration control loop module calculates the overall force deviation value of the multi-joint manipulator according to the difference between the acceleration command of the inertial sensing unit and the acceleration information detected by the inertial sensing unit, and decomposes the calculation through the force Jacobian matrix The joint force deviation value of each joint in the multi-stage robotic arm is based on the joint force deviation value and the force command of the previous cycle to generate the first force command of each joint.

The single joint control loop module 330 is configured to generate a second force command for each joint based on the point position command and the position information of each joint of the robot system. In one embodiment, the single joint control loop module 330 further includes:

The kinematics inverse solution module 331 is configured to generate the joint target position of each joint based on the point instruction. In one embodiment, the kinematics inverse solution module 331 calculates and generates the joint target position of each joint through a kinematics inverse solution algorithm according to the joint position command in the point position command.

The position control loop module 332 is configured to generate joint speed commands based on point commands and joint target positions. In one embodiment, the position control loop module 332 generates the joint speed command according to the difference between the joint target position and the joint position information, and the initial joint speed command.

The speed control loop module 333 is configured to generate a second force command based on the joint speed command and the joint position information. In one embodiment, the speed control loop module 333 generates the second force command according to the difference between the second joint speed command and the joint position information.

The force command fusion module 340 is configured to calculate and generate a fusion force command for each joint based on the first force command and the second force command. In one embodiment, the force command fusion module performs a weighted sum calculation on the first force command and the second force command based on a preset weight coefficient to generate a fusion force command.

The current control loop module 350 is configured to generate a current command for driving the drive unit according to the fusion force command.

In one embodiment, the control unit 300 may further include a collision judging module 360, which is used to judge whether the robot system collides according to the point command and acceleration information, and when it is judged that a collision occurs, send a message to the force command fusion module signal to modify the fusion power command; otherwise, the fusion power command is not modified.

In one embodiment, the control unit further includes a preset threshold value. When it is judged that a collision occurs, the collision judging module calculates an acceleration deviation value based on the point command and acceleration information, if any dimension value is greater than or equal to When the preset threshold is reached, send a stop operation command to the force command fusion module, set the fusion force command to 0, and stop the drive unit; otherwise, send a force hold command to the force command fusion module command, setting the fusion force command as the fusion force command of the previous cycle, so that the drive unit enters a force holding state.

In one embodiment, the control unit 300 may further include a storage 370 for storing preset parameters. In one embodiment, the storage 370 stores preset weight coefficients and preset thresholds.

In one embodiment, the control unit 300 may further include a communication preprocessing module 380, configured to preprocess the data input from the position sensor and the inertial sensing unit to the control unit 300, and store the preprocessed data in the communication preprocessing module 380. In the interface register of the processing module 380.

Fig. 2 shows a schematic flowchart of the control method of the robot system of the present invention.

The robot system includes: the robot body, including a multi-joint mechanical arm, and multiple drive units, used to drive each joint to turn or move; multiple position sensors, used to detect the joint position information of each joint; an inertial sensing unit, The inertial sensing unit is arranged at the front end of the multi-joint robot arm, and is used to detect the acceleration information of the inertial sensing unit at the setting position; the control unit is used to send the driving unit of each joint in the multi-joint robot system Output current command.

Control methods include:

Step S100, motion trajectory planning, generating point and position instructions according to user or other external instructions. In one embodiment, the point instruction includes the initial joint position instruction and the initial joint velocity instruction of each joint in the robot system in Cartesian space, and the inertial sensor unit speed instruction and inertial sensor unit setting position in Cartesian space. Sensing unit acceleration command.

Step S200, running the overall acceleration control loop step, used to generate the first force command of each joint based on the point command and the acceleration information detected by the inertial sensor unit of the robot system.

In one embodiment, step S200 further includes: step S210, calculating the overall force deviation value of the multi-joint manipulator according to the difference between the acceleration command of the inertial sensing unit and the acceleration information detected by the inertial sensing unit . Step S220, decompose and calculate the joint force deviation value of each joint in the multi-joint manipulator through the force Jacobian matrix. Step S230, generating a first force command for each joint based on the joint force deviation value and the force command of the previous cycle.

Step S300, running a single-joint control loop step for generating a second force command for each joint based on the point position command and the position information of each joint of the robot system.

In one embodiment, step S300 further includes: step S310, based on the point instruction, through a kinematics inverse solution algorithm, to generate the joint target position of each joint. Step S320, generating a joint speed command according to the difference between the joint target position and the joint position information, and the initial joint speed command. Step S330, generating a second force command based on the joint speed command and the joint position information.

Step S400, the force instruction fusion step, based on the first force instruction and the second force instruction, calculate and generate fusion force instructions for each joint. In one embodiment, the force command fusion module performs a weighted sum calculation on the first force command and the second force command based on a preset weight coefficient to generate a fusion force command.

Step S500, running a current control loop step, generating a current command for driving the driving unit based on the fusion force command.

In one embodiment, the control method further includes: step S600, a collision judgment step, judging whether the robot system has a collision according to the point instruction and the acceleration information, and if it is judged that a collision occurs, merge the force instruction with the The module sends out a signal to modify the fusion force command, and if no collision is determined, the fusion force command is not corrected.

In one embodiment, step S600, the collision judging step further includes, based on the acceleration information of the inertial sensing unit, calculating the acceleration information of each joint, and judging whether the directions of the point command and the acceleration information of each joint are the same, When the accelerations in any one of the dimensional directions are opposite, it is judged that there is a collision, otherwise it is judged that there is no collision.

In one embodiment, step S600, the collision judgment step further includes: calculating the acceleration deviation value based on the point command and the acceleration information of each joint, and if any dimension value is greater than or equal to a preset threshold value, the force command The fusion module sends a stop operation command, sets the fusion force command to 0, and stops the drive unit; otherwise, sends a force maintenance command to the force command fusion module, and sets the fusion force command to the previous Combined with the force command periodically, the drive unit enters into a force holding state.

Another preferred embodiment of the robot system and the control method of the robot system provided by the present invention include:

A six-joint industrial robot, comprising:

The robot body includes a six-joint robot arm and six drive units, the drive units include power devices, permanent magnet synchronous motors, harmonic reducers, etc., which are respectively arranged on each joint of the six-joint robot arm for driving Each joint turns or moves.

Six position sensors, in one embodiment, the position sensors are motor encoders, which are respectively arranged at each joint of the multi-joint manipulator, and are used to detect the joint position information of each joint.

An inertial sensing unit. In one embodiment, the inertial sensing unit is a dedicated circuit board installed at the fifth joint of the mechanical arm. The dedicated circuit board has an IO circuit, an inertial sensor and a microprocessor chip.

A control unit that outputs current commands to the drive units of each joint in the robot system according to the commands, including:

The motion planning module generates a series of point and position instructions according to a certain time interval. In one embodiment, the point and position instructions include:

a. The position, velocity and acceleration of the robot in Cartesian space.

b. The position and speed of each joint of the robot.

The overall acceleration control loop module is used to generate the first force command of each joint based on the point command and the acceleration information detected by the inertial sensor unit. In one embodiment, the overall acceleration control loop module operates according to the following mechanism :

The expected acceleration is obtained from the 6-dimensional acceleration command of the inertial sensor unit installation point in the Cartesian space in the point command, and the 6-dimensional acceleration information fed back by the inertial sensor is obtained. The difference between the two accelerations is calculated, and the acceleration difference is multiplied by the gain. As a Cartesian space force deviation in 6 dimensions. The force deviation value of each joint is obtained by multiplying the transposition of the Jacobian matrix and the 6-dimensional Cartesian space force deviation. The first force command of each joint is obtained by summing the force deviation value of each joint and the force command of the previous cycle. In one embodiment, the first force command of the joint whose position is closer to the end of the mechanical arm (executing mechanism) than the inertial sensing unit can be set to 0.

Because in this step, the overall acceleration control loop module does not need to find the inverse matrix of the Jacobian, the calculation amount is small, and it can meet the real-time control requirements.

A single-joint control loop module, configured to generate a second force command for each joint based on the point command and the position information of each joint of the robot system; in one embodiment, the single-joint control loop module includes:

A kinematics inverse solution module, configured to generate the joint target position of each joint based on the point instruction;

The position control loop module makes the difference between the joint position command and the motor encoder position feedback, multiplies it by the gain to obtain the speed command, and sends the speed command to the speed control loop module after adding the joint speed command;

The speed control loop module makes a difference between the above speed command and the position difference of the motor encoder, and multiplies it by the gain to obtain the second force command of each joint.

The force instruction fusion module is used to calculate and generate fusion force instructions for each joint based on the first force instruction and the second force instruction. In one embodiment, the force instruction fusion module performs the first force instruction and each The second force command of the joint is calculated based on the weighted summation of the preset weight coefficient to generate the fusion force command, that is, the formula: fusion force command = first force command * first weight coefficient + second force command * second weight coefficient is adopted.

A current control loop module, configured to generate a current command to drive the driving unit according to the fusion force command. In one embodiment, the current control loop module converts the fusion force instruction into a current instruction, and outputs a driving control signal (such as a PWM signal) to the power device (such as: IGBT, IPM, etc.) in the drive unit to drive the motor to run.

In one embodiment, the control unit may include a collision judging module, which is used to judge whether the robot system collides according to the acceleration instruction in the point instruction and the direction of the six-dimensional acceleration information detected by the inertial sensor unit. When the acceleration in the one-dimensional direction is opposite, it is judged that there is a collision, and a signal is sent to the fusion force command to modify the fusion force command; otherwise, it is judged that there is no collision, otherwise, the fusion force command is not corrected.

In one embodiment, the collision judging module calculates the acceleration deviation value based on the acceleration command in the point command and the six-dimensional acceleration information detected by the inertial sensing unit, and if any of the dimensional values is greater than or equal to the preset threshold value, it will report to the The force command fusion module sends a stop operation command, and the fusion force command is set to 0, so that the drive unit stops running; otherwise, it sends a force maintenance command to the force command fusion module, and the fusion force command is set to Combined with the force command from the previous cycle, the drive unit enters the force holding state.

In one embodiment, the core processing chip of the control unit can be a SOC-FPGA, Intel's CycloneV, which is integrated with a dual-core ARM Cortex-A9 processor and an FPGA processor. When the control unit is running, the ARM Cortex-A9 processor One core runs the operating system, responsible for non-real-time or weak real-time tasks such as network communication, human-computer interaction, file system management, and motion planning modules; the other core of the ARM Cortex-A9 processor runs strong real-time tasks, including the overall acceleration control loop module , Single joint control loop module, force command fusion module; FPGA processor includes current control loop module, etc.

In one embodiment, the control unit may include a storage module for storing preset parameters, such as: a first weight coefficient, a second weight coefficient, a preset threshold, and the like.

In one embodiment, the FPGA processor includes a communication preprocessing module, which preprocesses real-time communication data such as encoder and inertial sensor data, and puts the result in a designated interface register, saving the processing time of the dual-core ARM processor.

Another preferred embodiment of the robot system and the control method of the robot system provided by the present invention include

A six-joint collaborative lightweight robot, including:

The robot body contains integrated joints, and multiple drive units are integrated in each joint, including a 48V permanent magnet synchronous hollow motor and a harmonic reducer, which are used to drive each joint to turn or move.

An inertial sensing unit. In one embodiment, the inertial sensing unit includes an inertial sensor and an IO circuit, and is installed on the drive unit of the sixth joint (the joint closest to the end effector).

A control unit, the control unit includes a plurality of microprocessors, respectively arranged at the drive unit of each joint, used to generate PWM signals to control the drive unit, obtain encoder information and current information, the microprocessor of the sixth joint drive unit Also responsible for obtaining inertial sensor information.

a. The position, velocity and acceleration of the robot in Cartesian space.

b. The position and speed of each joint of the robot.

The overall acceleration control loop module is used to generate the first force command of each joint based on the point command and the acceleration information detected by the inertial sensor. In one embodiment, the overall acceleration control loop module operates according to the following mechanism:

The expected acceleration is obtained from the 6-dimensional acceleration command of the inertial sensor unit installation point in the Cartesian space in the point command, and the 6-dimensional acceleration information fed back by the inertial sensor is obtained. The difference between the two accelerations is calculated, and the acceleration difference is multiplied by the gain. As a Cartesian space force deviation in 6 dimensions. The force deviation value of each joint is obtained by multiplying the transposition of the Jacobian matrix and the 6-dimensional Cartesian space force deviation. The first force command of each joint is obtained by summing the force deviation value of each joint and the force command of the previous cycle.

In one embodiment, the core processing chip of the control unit can be a SOC-FPGA, Intel's CycloneV, which is integrated with a dual-core ARM Cortex-A9 processor and an FPGA processor. When the control unit is running, the ARM Cortex-A9 processor One core runs the operating system, which is responsible for non-real-time or weak real-time tasks such as network communication, human-computer interaction, file system management, and motion planning modules; the other core of the ARM Cortex-A9 processor runs strong real-time tasks, including the overall acceleration control loop module , Single joint control loop module, force command fusion module; FPGA processor includes current control loop module, etc.

It can be known from the above embodiments that the robot system and the control method of the robot system provided by the present invention realize the control of the multi-joint manipulator through the comprehensive application of the overall acceleration control loop, the single-joint control loop and the current control loop, and can hardly increase the In the case of low robot system cost, it can improve the terminal shaking phenomenon during the operation of the industrial robot and improve the working efficiency of the robot system; through the acceleration deviation and direction judgment, it can sensitively detect any joint in the multi-joint manipulator during operation. Collision, to avoid misjudgment, and after the collision is detected, enter the force holding state or stop operation state in time, thereby reducing the risk of accidents; in each control loop, there is no need to perform any matrix inversion operation, the control loop The calculation amount of the road is small, which is conducive to improving the real-time performance of the robot system as a whole; by detecting and comparing the acceleration of the front end of the multi-joint manipulator, and decomposing it into the control loop of each joint, there is no need to calculate the acceleration information from the speed information difference, avoiding In the differential process, the noise contained in the detected speed information is further amplified, thereby enhancing the robustness of the robot system.

The terms and expressions used here are for description only, and the present invention should not be limited to these terms and expressions. The use of these terms and expressions does not mean to exclude any equivalent features shown and described (or parts thereof), and it should be recognized that various modifications may also be included within the scope of the claims. Other modifications, changes and substitutions are also possible. Accordingly, the claims should be read to cover all such equivalents.

Similarly, it should be pointed out that although the present invention has been described with reference to the current specific embodiments, those of ordinary skill in the art should recognize that the above embodiments are only used to illustrate the present invention, without departing from the present invention. Various equivalent changes or substitutions can also be made under the spirit of the present invention. Therefore, as long as the changes and modifications to the above embodiments are within the spirit of the present invention, they will all fall within the scope of the claims of the present application.

Claims

A control unit of a robot system, configured to output a current command to a drive unit of each joint in the robot system according to an instruction, wherein the control unit includes:

A motion planning module generates a point instruction according to the instruction, which is used for the motion trajectory planning of the robot system;

The overall acceleration control loop module is used to generate the first force command of each joint based on the point command and the acceleration information of any point in the robot system;

A single-joint control loop module, configured to generate a second force command for each joint based on the point position command and the position information of each joint of the robot system;

A force command fusion module, configured to calculate and generate a fusion force command for each joint based on the first force command and the second force command;

A current control loop module, configured to generate a current command to drive the driving unit according to the fusion force command.
The control unit according to claim 1, wherein the single-joint control loop module comprises:

A kinematics inverse solution module, configured to generate the joint target position of each joint based on the point instruction;

A position control loop module, configured to generate a joint speed command based on the point command and the joint target position;

A speed control loop module, configured to generate a second force command based on the joint speed command and the joint position information.
The control unit according to claim 1, wherein the control unit further comprises: a collision judging module, configured to judge whether the robot system collides according to the point instruction and the acceleration information, and when judging When a collision occurs, a signal is sent to the force command fusion module to modify the fusion force command; otherwise, the fusion force command is not corrected.
The control unit according to claim 3, characterized in that, the control unit further includes a preset threshold, and when it is judged that a collision occurs, the collision judging module calculates the acceleration of each joint based on the point command and the acceleration information Deviation value, if any dimension value is greater than or equal to the preset threshold value, send a stop operation instruction to the force instruction fusion module, set the fusion force instruction to 0, and stop the drive unit; otherwise , then send a force maintenance command to the force command fusion module, set the fusion force command as the fusion force command of the previous cycle, and make the drive unit enter a force maintenance state.
The control unit according to any one of claims 1 to 4, wherein the control unit is integrated in a chip, and the chip is integrated with a dual-core or multi-core processor and an FPGA processor, and the control unit operates ,

The motion planning module runs on one core of the dual-core or multi-core processor; the overall acceleration control loop module, the single-joint control loop module, and the force command fusion module run on the other core of the dual-core or multi-core processor. One core runs; the current control loop module runs on the FPGA processor.
A robotic system comprising:

The robot body includes a multi-joint mechanical arm, and a plurality of drive units, the number of the drive units corresponds to the number of joints of the multi-joint mechanical arm, and they are respectively arranged on each joint of the multi-joint mechanical arm for Drive each joint to turn or move;

A plurality of position sensors, the number of the position sensors corresponds to the number of joints of the multi-joint manipulator, which are respectively arranged at each joint of the multi-joint manipulator, and are used to detect the joint position information of each joint;

An inertial sensing unit, the inertial sensing unit is arranged on the multi-joint mechanical arm, and is used to detect the acceleration information of the installation position of the inertial sensing unit;

It is characterized in that,

The robot system comprises the control unit according to any one of claims 1-4.
The robot system according to claim 6, wherein the point instructions include initial joint position instructions, initial joint velocity instructions, and position instructions, velocity instructions, and accelerations in Cartesian space of each joint in the robot system. instruction.
The robot system according to claim 7, wherein the overall acceleration control loop module calculates the overall acceleration of the multi-joint manipulator according to the difference between the acceleration command in the Cartesian space and the acceleration information. The force deviation value and the joint force deviation value of each joint, the first force command of each joint is calculated and generated based on the joint force deviation value and the force command of the previous cycle.
A control method of a robot system, which is used to output a current command to a drive unit of each joint in the robot system according to an instruction, wherein the control method includes:

In the motion trajectory planning step, a point command is generated according to the command;

Running the overall acceleration control loop step, generating a first force command for each joint based on the point command and the acceleration information of any point in the robot system;

Running the single joint control loop step, generating a second force command for each joint based on the point command and the position information of each joint of the robot system;

The force command fusion step is to calculate and generate fusion force commands for each joint based on the first force command and the second force command;

Running a current control loop step to generate a current command for driving the drive unit based on the fusion force command.
The control method of the robot system as claimed in claim 9, is characterized in that,

The robot system includes: a robot body, including a multi-joint mechanical arm, and a plurality of drive units, used to drive each joint to turn or move; a plurality of position sensors, used to detect the joint position information of each joint; an inertial sensor A unit, the inertial sensing unit is arranged on the multi-joint mechanical arm, and is used to detect the acceleration information of the installation position of the inertial sensing unit;

Wherein the point instruction includes the initial joint position instruction, the initial joint velocity instruction and the position instruction, velocity instruction and acceleration instruction in Cartesian space of each joint in the robot system.
The control method of the robot system as claimed in claim 10, is characterized in that,

The steps of running the overall acceleration control loop include:

According to the difference between the acceleration command in the Cartesian space and the acceleration information, calculate the overall force deviation value of the multi-joint manipulator and the joint force deviation value of each joint, and the first force command of each joint is based on The joint force deviation value and the force command of the previous cycle are calculated and generated.
The control method of the robot system according to claim 11, wherein the step of running the single-joint control loop comprises:

In the kinematics inverse solution step, based on the point instruction, the joint target position of each joint is generated;

The step of operating the position control loop is to generate a joint speed command based on the point command and the joint target position;

The step of running the speed control loop is to generate a second force command based on the joint speed command and the joint position information.
The control method of the robot system as claimed in claim 9, is characterized in that, described control method also comprises:

The collision judging step is to judge whether the robot system has a collision according to the point instruction and the acceleration information, and if it is judged that a collision occurs, send a signal to the force instruction fusion module to correct the fusion force instruction, if it is judged that there is no collision Collision occurs, does not modify said fusion force command.
The control method of the robot system according to claim 13, characterized in that, said collision judging step also includes, based on said acceleration information in Cartesian space, calculating the acceleration information of each joint, and judging the relationship between said point command and Whether the directions of the acceleration information of the joints are the same, if the acceleration in any one of the dimensional directions is opposite, it is judged that a collision has occurred, otherwise, it is judged that there has been no collision.
The control method of the robot system according to claim 14, characterized in that,

The collision judging step includes: calculating an acceleration deviation value based on the point command and the acceleration information of each joint, and if any dimension value is greater than or equal to a preset threshold value, sending a stop operation command to the force command fusion module, and The fusion force command is set to 0, so that the drive unit stops running; otherwise, a force maintenance command is sent to the force command fusion module, and the fusion force command is set as the fusion force command of the previous cycle, so that all The drive unit enters the force holding state.