A multi-arm mobile robot control system and a control method thereof

Abstract

The application relates to a multi-arm mobile robot control system, which comprises a robot, each joint position of the mechanical arm of which is configured with a joint controller for driving joint movement; a robot controller for realizing the operation of joint motors according to control quantities; a joint position hard analysis unit for calculating the coordinate positions of each joint or mechanical arm in a space coordinate system to output results; and a joint space point position trajectory constraint model for forming a correction quantity of a joint and superimposing the correction quantity on an original control quantity of the joint to become a final control quantity for controlling the joint, the final control quantity being used for output to the joint controller to realize joint movement. The application provides a multi-arm mobile robot control system, which effectively realizes the cooperative control among multi-arm robots, avoids the mutual collision among mechanical arms, improves the avoidance and dynamic response speed of the mechanical arms, and improves the agility of robot actions.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a control system and control method for a multi-arm mobile robot. Background Technology

[0002] With the increasing application of mobile robots in industrial settings, especially in manufacturing, hybrid robots, often carrying multi-degree-of-freedom robotic arms on mobile chassis, offer greater efficiency and flexibility in manufacturing, becoming a key component of the flexibility of automated production lines. Mobile robots with multiple arms, such as two or four arms, offer even greater operational flexibility and efficiency than single-arm robots.

[0003] Taking a four-armed robot as an example, there are technical challenges such as the coordinated control of the four arms and collision avoidance among them. Without resolving these issues, the advantages of multi-armed robots cannot be fully realized, and they cannot be truly applied in manufacturing scenarios. Therefore, with the rapid development of robot control technology and artificial intelligence technology, and the increasing demand for high-efficiency robot operations in manufacturing scenarios, the industry urgently needs an effective four-armed robot control strategy to solve the problems of coordinated control among the four arms and mutual collision avoidance. Summary of the Invention

[0004] Therefore, in order to further improve the safety, real-time response speed, and reliability of the collaborative control among the four arms in a practical system, this invention proposes a real-time collision avoidance control technology based on hard analysis of joint positions, which further enhances the collision avoidance capability of the four-arm collaborative control and improves the response speed, safety, and reliability.

[0005] To address the aforementioned technical problems, this invention provides a multi-arm mobile robot control system, comprising: a robot having at least two robotic arms, wherein each robotic arm is a multi-axis robotic arm, and each joint of the multi-axis robotic arm is equipped with a joint controller for driving joint movement; a robot controller that communicates and interacts with the joint controller, the robot controller sending control quantities to the joint controller to enable the joint controller to execute the operation of the joint motors according to the control quantities; a joint position hard analysis unit that calculates the coordinate positions of each joint or robotic arm in a spatial coordinate system based on the forward kinematics model of the robotic arm and outputs the results; and a joint spatial point trajectory constraint model that receives the output results of the joint position hard analysis unit as input, the joint spatial point trajectory constraint model forming a correction quantity for the joint and superimposing it on the original control quantity of the joint to become the final control quantity for controlling the joint, the final control quantity being output to the joint controller to realize joint movement.

[0006] In one embodiment of the present invention, a comparison monitoring module is provided between the joint position hard resolution unit and the robot controller for comparing the parameters of the same joint. The comparison monitoring module is used to compare the control quantity of the robot controller and the output result of the joint position hard resolution unit for the same joint.

[0007] In one embodiment of the present invention, the control system further includes a reinforcement learning unit, which receives the final control quantity of the joint space point trajectory constraint model, optimizes it, and outputs the optimized value to the robot controller.

[0008] In one embodiment of the present invention, the input to the joint position hard resolution unit is the reading of the encoder at the joint and the DH parameter at this joint.

[0009] In one embodiment of the present invention, the constraint conditions of the joint space point trajectory constraint model include: ; ; ; ;

[0010] in, Let be the velocity of the j-th joint of the i-th robotic arm;

[0011] Joint speed limitation;

[0012] Let be the acceleration of the j-th joint of the i-th robotic arm;

[0013] Limiting joint acceleration;

[0014] Let be the angular acceleration of the j-th joint of the i-th robotic arm;

[0015] Joint angular acceleration limitation;

[0016] Let be the angular velocity of the j-th joint of the i-th robotic arm;

[0017] This is for joint angular velocity limitations.

[0018] In one embodiment of the present invention, the collision constraint condition between any two joints of different robotic arms on the robot is as follows:

[0019] ;

[0020] ;

[0021] ;

[0022] in, Let i be the i-th joint of the α-th robotic arm at time K.

[0023] Let j be the j-th joint of the β-th robotic arm at time K, where β ≠ α;

[0024] Let xy, yz, xz be the Euclidean minimum distance in the two-dimensional plane.

[0025] In one embodiment of the present invention, the constraint condition between the joint changes of different robotic arms on the robot is as follows:

[0026] ;

[0027] ;

[0028] ;

[0029] Furthermore, the constraint is set as follows: within a certain time period t1-t2,

[0030] ;

[0031] ;

[0032] ;

[0033] in, The minimum Euclidean distance setting for the joints of different robotic arms in the xy plane;

[0034] The minimum Euclidean distance setting for the joints of different robotic arms in the yz plane;

[0035] This is the minimum Euclidean distance setting for the joints of different robotic arms in the xz plane.

[0036] The present invention also includes a control method for a multi-arm mobile robot control system, comprising the following steps:

[0037] S1. The robot controller generates motion control trajectory instructions for the robotic arm, that is, the spatial position coordinates (x, y, z) that each joint in the robot needs to reach.

[0038] S2. Input the spatial position coordinates (x, y, z) to be reached by each joint in step S1 into the joint position hard analysis unit. The joint position hard analysis unit performs spatial position calibration mapping on the spatial position coordinates (x, y, z) to form a spatial matrix data that can represent the spatial position relationship between each joint.

[0039] S3. Output the spatial matrix data to the joint spatial point trajectory constraint model, correct the input data through the constraint relationship in the joint spatial point trajectory constraint model, and output the corrected final control quantity to the joint controller.

[0040] S4. The joint controller drives the movement of each joint based on the final control value.

[0041] In one embodiment of the present invention, in step S3, the constraint relationship in the joint space point trajectory constraint model is that the spatial motion of each joint has two constraints: the spatial constraint of the trajectory planning path of the robotic arm where the joint is located and the constraint of the mutual collision avoidance constraint model of each robotic arm in space.

[0042] In one embodiment of the present invention, in step S3, when the continuous Euclidean distance on the three planes in the (x,y,z) coordinate system is the smallest, and the change in Euclidean distance on the corresponding plane is close to or has reached a certain negative value range, when this condition is met and the step is triggered, the correction amount of the i-th joint on the corresponding plane is calculated and superimposed on the original control amount to become the final control amount output.

[0043] The multi-arm mobile robot control system and control method of the present invention have the following advantages compared with the prior art:

[0044] 1. Effectively achieves mutual avoidance and dynamic response speed between multi-armed mobile robots, improving the agility of robot movements;

[0045] 2. It solves the deadlock problem between multi-armed mobile robots at the underlying control level, making robot collaborative actions more reliable;

[0046] 3. To make the collaborative operation of the robotic arms of multi-arm mobile robots more efficient. Attached Figure Description

[0047] To more clearly illustrate the technical solutions of the embodiments of this application, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0048] Figure 1 The present invention relates to a multi-arm mobile robot control system and its control method. Figure 1 ;

[0049] Figure 2 The present invention relates to a multi-arm mobile robot control system and its control method. Figure 2 ;

[0050] Figure 3 The present invention relates to a multi-arm mobile robot control system and its control method. Figure 3 ;

[0051] Figure 4 The present invention relates to a multi-arm mobile robot control system and its control method. Figure 4 ;

[0052] Figure 5 This is a control diagram of the multi-arm mobile robot control system and its control method of the present invention.

[0053] Explanation of reference numerals in the accompanying drawings: Robot controller 10, joint position hard analysis unit 20, joint space point trajectory constraint model 30, comparison and monitoring module 40, reinforcement learning unit 50. Detailed Implementation

[0054] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments described are not intended to limit the present invention.

[0055] Reference Figure 1As shown, the multi-arm mobile robot control system of the present invention includes: a robot, a robot controller 10, a joint position hard resolution unit 20, and a joint space point trajectory constraint model 30. The robot has at least two robotic arms, and each robotic arm is a multi-axis robotic arm. Each joint position of the multi-axis robotic arm is equipped with a joint controller to drive the joint movement. The robot controller 10 communicates with the joint controller via EtherCat, and the robot controller 10 sends control signals to the joint controller to enable the joint controller to execute the joint motor operation according to the control signals. The joint position hard resolution unit 20 is specifically designed using an FPGA. The ASIC is the main processing core of the hardware computing unit. The input of the joint position hard analysis unit 20 is the encoder reading at the joint and the DH parameter at this joint. Then, the joint position hard analysis unit 20 calculates the coordinate position of each joint or the robot arm in the spatial coordinate system based on the forward kinematics model of the robot arm and outputs the result. The joint spatial point trajectory constraint model 30 receives the output result of the joint position hard analysis unit 20 as input. The joint spatial point trajectory constraint model 30 is used to form the correction amount of the joint and superimpose it on the original control amount of the joint to become the final control amount of the joint. The final control amount is used to output to the joint controller to realize the joint movement.

[0056] Based on the above scheme, a comparison monitoring module 40 for comparing parameters of the same joint is provided between the joint position hard analysis unit 20 and the robot controller 10, such as... Figure 2 As shown, the comparison monitoring module 40 is used to compare the control input of the robot controller 10 at the same joint with the output of the joint position hard analysis unit 20. Specifically, the "position coordinate data of all joint axes" obtained by the joint position hard analysis unit 20 and the "position coordinates of all joint axes" obtained by the robot controller 10 should theoretically be consistent, since the former is calculated by the joint position hard analysis unit 20, while the latter is calculated by the robot controller 10 according to the program software algorithm. However, in this invention, the joint position hard analysis unit 20 is based on hardware parallel computing, which has higher real-time performance. Therefore, the calculation results of the joint position hard analysis unit 20 and the calculation results of the robot controller 10 have a certain deviation on the time axis. This deviation is within a certain range and is considered normal. However, if the deviation exceeds a certain range, it indicates that there is a problem with a certain joint, and an alarm can be sent to the robot controller 10. Therefore, the comparison monitoring module 40 here can ensure that the calculated joint spatial position coordinates are more reliable and more real-time.

[0057] Based on the above scheme, the spatial motion of each joint in the joint spatial point trajectory constraint model 30 is subject to two constraints: one is the spatial constraint of the trajectory planning path of the robotic arm to which it is located; the other is the constraint of the spatial collision avoidance constraint model of multiple robotic arms. Under these two constraints, the motion of any joint will generate collision avoidance space with other joints (joints located on other arms that are spatially adjacent to it) to avoid the possibility of collision.

[0058] The position coordinates of a joint in space also include the physical dimensions of the joint. For clarity and simplicity, the position coordinates of a joint are described using the coordinate data of the center of the joint axis. Therefore, the constraints on the trajectory constraint model (30) of any joint in joint space include: ; ; ; ;

[0059] in, Let be the velocity of the j-th joint of the i-th robotic arm;

[0060] Joint speed limitation;

[0061] Let be the acceleration of the j-th joint of the i-th robotic arm;

[0062] Limiting joint acceleration;

[0063] Let be the angular acceleration of the j-th joint of the i-th robotic arm;

[0064] Joint angular acceleration limitation;

[0065] Let be the angular velocity of the j-th joint of the i-th robotic arm;

[0066] This is for joint angular velocity limitations.

[0067] Let be the position coordinates of the j-th joint of the i-th robotic arm in the unified coordinate system at time k.

[0068] The collision constraints between any two joints of different robotic arms on the robot are as follows:

[0069] ;

[0070] ;

[0071] ;

[0072] in, Let i be the i-th joint of the α-th robotic arm at time K.

[0073] Let j be the j-th joint of the β-th robotic arm at time K, where β ≠ α;

[0074] Let xy, yz, xz be the Euclidean minimum distance in the two-dimensional plane.

[0075] Furthermore, the constraints between the joint changes of different robotic arms on the robot are as follows:

[0076] ;

[0077] ;

[0078] ;

[0079] Furthermore, the constraint is set as follows: within a certain time period t1-t2, the sum of the changes in Euclidean distance must be greater than a set value. That is, the minimum value allowed in the xy plane; the same applies to the yz plane and the xz plane.

[0080] ;

[0081] ;

[0082] ;

[0083] in, The minimum Euclidean distance setting for the joints of different robotic arms in the xy plane;

[0084] The minimum Euclidean distance setting for the joints of different robotic arms in the yz plane;

[0085] The minimum Euclidean distance setting for the joints of different robotic arms in the xz plane;

[0086] During this time period, if the sum of the changes in Euclidean distance is positive and the larger the value, the higher the safety margin of the two joints in the direction of movement; otherwise, if the value is negative and below a certain value, it indicates that the two joints tend to be at a dangerous distance in the direction of movement.

[0087] Therefore, the sum of Euclidean changes within a certain time period t1~t2 (the above three expressions, respectively representing the xy, yz, and xz planes) can be regarded as three activation functions in reinforcement learning; the larger the value, the more positive the activation function, and the more positive the activation function; the smaller the value, the more negative the activation function, and the more negative the activation function. Based on this, reinforcement learning further optimizes the robot trajectory.

[0088] Therefore, based on the above scheme, a reinforcement learning unit 50 is introduced, and a control system for the reinforcement learning unit 50 is added, such as... Figure 3 The reinforcement learning unit 50 receives the final control quantity of the joint space point trajectory constraint model 30, optimizes it, and outputs the optimized value to the robot controller 10.

[0089] like Figure 4 As shown, the final control quantity, or "final command," for the robot's motion control trajectory is to directly and in real-time correct the pose of one of the two joints located on different robotic arms that are spatially closest to each other, ensuring that no collision occurs. This can be seen as implementing a function similar to "elastic mutual exclusion," preventing collisions between joints that are spatially adjacent between any two robotic arms.

[0090] When the continuous Euclidean distances on the xy, yz, and xz planes are minimized, and the changes in Euclidean distances on the corresponding planes are close to or have reached a certain negative range, when this condition is triggered, the correction amount of the control quantity of the i-th joint on a certain plane is calculated and superimposed on the original control quantity to become the final control quantity output (final command). Simultaneously, this information is returned to the robot controller 10 to eliminate or compensate for problems caused by directly correcting a segment of control command. The general rule is to correct the control quantity of a joint with a high motion speed, but other rules can also be set. Figure 4 middle, For the control quantity of the i joint located on the α arm, This is the adjustment value for the control quantity of joint i on arm α; + This represents the corrected control quantity at joint i on arm α. The final overall system is formed through the analysis of the final control quantities of the robot's motion control trajectory. Figure 5 As shown.

[0091] Based on the above system, a control method for a multi-arm mobile robot control system includes the following steps:

[0092] S1. The robot controller generates motion control trajectory instructions for the robotic arm, that is, the spatial position coordinates (x, y, z) that each joint in the robot needs to reach.

[0093] S2. Input the spatial position coordinates (x, y, z) to be reached by each joint in step S1 into the joint position hard analysis unit. The joint position hard analysis unit performs spatial position calibration mapping on the spatial position coordinates (x, y, z) to form a spatial matrix data that can represent the spatial position relationship between each joint.

[0094] S3. Output the spatial matrix data to the joint spatial point trajectory constraint model, correct the input data through the constraint relationship in the joint spatial point trajectory constraint model, and output the corrected final control quantity to the joint controller.

[0095] S4. The joint controller drives the movement of each joint based on the final control value.

[0096] In step S3, when the continuous Euclidean distance on the three planes in the (x,y,z) coordinate system is the smallest, and the change in Euclidean distance on the corresponding plane is close to or has reached a certain negative value range, the correction amount of the i-th joint on the corresponding plane is calculated and superimposed on the original control amount to become the final control output when this condition is met and triggered.

[0097] Specifically, taking a four-armed mobile robot, with each arm having six axes, as an example, the four arms have a total of 24 axes. Each servo joint is equipped with a servo joint motor driver, also known as a joint controller, resulting in 24 joint controllers for the four arms. The robot controller 10 generates motion control trajectory commands for the robotic arms, i.e., the spatial position coordinates (x, y, z) to be reached by each joint; the four arms have a total of 24 sets of position coordinates. The 24 sets of joint positions in space are used as 24 controllable point targets, which are input to the joint position hard analysis unit 20. The joint position hard analysis unit 20 performs spatial position calibration mapping on the position data of these multiple points, forming a spatial matrix data that can represent the spatial relationship between positions. This matrix data is output to the joint spatial point trajectory constraint model 30. In this model, a constraint model containing the physical dimensions of relevant joint points and their mutual spatial position relationships is established. Under the constraints of this model, the original input "initial command" is corrected, and the corrected "final command" is output to the joint controller.

[0098] Specifically, the joint spatial point trajectory constraint model 30 defines the spatial position interference relationships of the 24 axes and the positional boundaries including the physical dimensions of the joints. Using the overall spatial operating range as a constraint and the kinematics and dynamics of the robotic arm as the model basis, it controls the adjustment of each joint in real time to avoid collisions and deadlocks.

[0099] Therefore, this invention proposes a four-arm cooperative control system to achieve four-arm cooperative control, avoid collisions, and avoid deadlocks.

[0100] Obviously, the above embodiments are merely illustrative examples for clear explanation and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. A control system for a multi-armed mobile robot, characterized in that, include: A robot having at least two robotic arms, wherein the robotic arms are multi-axis robotic arms and each joint of the multi-axis robotic arm is equipped with a joint controller for driving the joint movement; A robot controller that communicates and exchanges data with a joint controller. The robot controller sends control signals to the joint controller so that the joint controller can execute the operation of the joint motors according to the control signals. The joint position hard analytical unit is based on the forward kinematics model of the robot arm and calculates the coordinate position of each joint or robot arm in the spatial coordinate system to output the result; The joint spatial point trajectory constraint model receives the output of the joint position hard analysis unit as input. The joint spatial point trajectory constraint model is used to form the correction amount of the joint and superimpose it on the original control amount of the joint to become the final control amount of the joint. The final control amount is used to output to the joint controller to realize the joint movement.

2. The multi-arm mobile robot control system according to claim 1, characterized in that: A comparison monitoring module is provided between the joint position hard resolution unit and the robot controller to compare the parameters of the same joint. The comparison monitoring module is used to compare the control quantity of the robot controller and the output result of the joint position hard resolution unit for the same joint.

3. The multi-arm mobile robot control system according to claim 1 or 2, characterized in that: The control system further includes a reinforcement learning unit, which receives the final control quantity of the joint space point trajectory constraint model, optimizes it, and outputs the optimized value to the robot controller.

4. The multi-arm mobile robot control system according to claim 1, characterized in that: The input to the joint position hard resolution unit is the encoder reading at the joint and the DH parameter at this joint.

5. The multi-arm mobile robot control system according to claim 3, characterized in that: The constraints of the joint space point trajectory constraint model include: ; ; ; ; in, Let be the velocity of the j-th joint of the i-th robotic arm; Joint speed limitation; Let be the acceleration of the j-th joint of the i-th robotic arm; Limiting joint acceleration; Let be the angular acceleration of the j-th joint of the i-th robotic arm; Joint angular acceleration limitation; Let be the angular velocity of the j-th joint of the i-th robotic arm; This is for joint angular velocity limitations.

6. The multi-arm mobile robot control system according to claim 5, characterized in that: The collision constraint conditions between any two joints of different robotic arms on the robot are as follows: ； ； ； in, Let i be the i-th joint of the α-th robotic arm at time K. Let j be the j-th joint of the β-th robotic arm at time K, where β ≠ α; Let xy, yz, xz be the Euclidean minimum distance in the two-dimensional plane.

7. The multi-arm mobile robot control system according to claim 6, characterized in that: The constraint condition between the joint changes of different robotic arms on the robot is: ； ； ； Furthermore, the constraint is set as follows: within a certain time period t1-t2, ; ; ; in, The minimum Euclidean distance setting for the joints of different robotic arms in the xy plane; The minimum Euclidean distance setting for the joints of different robotic arms in the yz plane; This is the minimum Euclidean distance setting for the joints of different robotic arms in the xz plane.

8. The control method for the multi-arm mobile robot control system according to any one of claims 1-7, characterized in that: Includes the following steps: S1. The robot controller generates motion control trajectory instructions for the robotic arm, that is, the spatial position coordinates (x, y, z) that each joint in the robot needs to reach. S2. Input the spatial position coordinates (x, y, z) to be reached by each joint in step S1 into the joint position hard analysis unit. The joint position hard analysis unit performs spatial position calibration mapping on the spatial position coordinates (x, y, z) to form a spatial matrix data that can represent the spatial position relationship between each joint. S3. Output the spatial matrix data to the joint spatial point trajectory constraint model, correct the input data through the constraint relationship in the joint spatial point trajectory constraint model, and output the corrected final control quantity to the joint controller. S4. The joint controller drives the movement of each joint based on the final control value.

9. The control method for the multi-arm mobile robot control system according to claim 8, characterized in that: In step S3, the constraint relationship in the joint spatial point trajectory constraint model is that the spatial motion of each joint has two constraints: the spatial constraint of the trajectory planning path of the robotic arm where the joint is located and the constraint of the mutual collision avoidance constraint model of each robotic arm in space.

10. The control method for the multi-arm mobile robot control system according to claim 9, characterized in that: In step S3, when the continuous Euclidean distance on the three planes in the (x,y,z) coordinate system is the smallest, and the change in Euclidean distance on the corresponding plane is close to or has reached a certain negative value range, the correction amount of the i-th joint on the corresponding plane is calculated and superimposed on the original control amount to become the final control output when this condition is met and triggered.

Images