Multi-arm mobile robot control system and control method thereof

Abstract

The invention relates to a multi-arm mobile robot control system, which comprises a robot, and a joint controller for driving a joint to move is arranged at each joint position of a mechanical arm of the robot; the robot controller is used for enabling the joint controller to execute operation of the joint motor according to the control quantity; the joint position hard analysis unit is used for calculating the coordinate position of each joint or the mechanical arm in a space coordinate system so as to output a result; and the joint space point locus constraint model is used for forming the correction amount of the joint and superposing the correction amount to the original control amount of the joint to form the final control amount for controlling the joint, and the final control amount is used for being output to the joint controller to realize joint movement. According to the multi-arm mobile robot control system provided by the invention, cooperative control among multi-arm robots is effectively realized, mutual collision among mechanical arms is avoided, the avoidance and dynamic response speed of the mechanical arms is increased, and the flexibility of robot actions is improved.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a multi-arm mobile robot control system and a control method thereof. Background Art

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

[0003] Taking a four-arm robot as an example, there are technical challenges such as coordinated control of the four arms and mutual collision avoidance. Without resolving these technical issues, the advantages of multi-arm 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, as well as the demand for high-efficiency robot operation in manufacturing scenarios, the industry urgently needs an effective four-arm robot control strategy to solve the problems of coordinated control and mutual collision avoidance among the four arms. Summary of the Invention

[0004] To this end, in the actual system, in order to further improve the safety, real-time response speed, and reliability of the coordinated control among the four arms, the present invention proposes a real-time collision avoidance control technical solution with hard analysis of joint positions, which further enhances the collision avoidance capability of the four-arm coordinated control and improves the response speed, safety, and reliability.

[0005] In order to solve the above technical problems, the present invention provides a multi-arm mobile robot control system, comprising: a robot, which is provided with at least two robotic arms, and the robotic arms are multi-axis robotic arms, and each joint position of the multi-axis robotic arms is configured with a joint controller for driving the joint movement; a robot controller, which communicates and exchanges data with the joint controller, and the robot controller is used to send a control amount to the joint controller so that the joint controller executes the operation of the joint motor according to the control amount; a joint position hard analysis unit, which is based on the forward kinematics model of the robotic arm, and calculates the coordinate position of each joint or robotic arm in the spatial coordinate system to output the result; a joint space point trajectory constraint model, which receives the output result of the joint position hard analysis unit as input, and the joint space point trajectory constraint model is used to form a correction amount for the joint and superimpose it on the original control amount of this joint to become the final control amount for controlling this joint, and the final control amount is used to output to the joint controller to realize joint movement.

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

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

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

[0009] In one embodiment of the present invention, the constraint conditions of the joint space point trajectory constraint model include: ; ; ; ; in, is the velocity of the jth joint of the i-th robotic arm; is the joint speed limit; is the acceleration of the jth joint of the i-th robotic arm; is the joint acceleration limit; is the angular acceleration of the jth joint of the i-th robotic arm; is the joint angular acceleration limit; is the angular velocity of the jth joint of the i-th robotic arm; is the joint angular velocity limit.

[0010] In one embodiment of the present invention, the collision constraint conditions between two joints of different robotic arms of the robot are: ; ; ; in, is the i-th joint of the α-th manipulator at the K-th moment; is the jth joint of the βth manipulator at the Kth moment, β≠α; is the Euclidean minimum distance in the xy, yz, xz two-dimensional plane.

[0011] In one embodiment of the present invention, the constraint conditions between the joint variations of different robotic arms on the robot are: ; ; ; Furthermore, the constraint conditions are set as follows: within a certain time period t1-t2, ; ; ; in, Set the minimum Euclidean distance between two joints of different manipulators in the xy plane; Set the minimum Euclidean distance between two joints of different manipulators in the yz plane; Set the minimum Euclidean distance between two joints of different manipulators in the xz plane.

[0012] The present invention also includes a control method for a multi-arm mobile robot control system, comprising the following steps: S1, the robot controller generates the motion control trajectory instructions of the robot 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) of each joint to be reached in step S1 into the joint position hard parsing unit. The joint position hard parsing unit performs spatial position calibration and mapping on the spatial position coordinates (x, y, z) to form spatial matrix data that can represent the relationship between the spatial positions of each joint; S3, outputting the spatial matrix data to the joint space point trajectory constraint model, correcting the input data through the constraint relationship in the joint space point trajectory constraint model, and outputting the corrected final control amount to the joint controller; S4. The joint controller drives the movement of each joint according to the final control amount.

[0013] In one embodiment of the present invention, in step S3, the constraint relationship in the joint space point trajectory constraint model is that in the spatial movement of each joint, there are two constraint conditions, namely: the spatial constraint of the trajectory planning path of the robotic arm where the joint itself is located and the constraint of the mutual collision avoidance constraint model of each robotic arm in space.

[0014] In one embodiment of the present invention, in step S3, when the continuous Euclidean distances on the three planes in the (x, y, z) coordinate system are minimized, and the change in the Euclidean distance on the corresponding plane is close to or has reached a certain negative range, when this condition is met and 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.

[0015] The multi-arm mobile robot control system and control method of the present invention have the following advantages over the prior art: 1. Effectively realize mutual avoidance and dynamic response speed between multi-arm mobile robots, and improve the agility of robot movements; 2. It solves the deadlock between multi-arm mobile robots at the bottom control level, making the robots' coordinated movements more reliable; 3. Make the collaborative operation of the robotic arms of multi-arm mobile robots more efficient. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0017] Figure 1 The multi-arm mobile robot control system and control method of the present invention are Figure 1 ; Figure 2 The multi-arm mobile robot control system and control method of the present invention are Figure 2 ; Figure 3 The multi-arm mobile robot control system and control method of the present invention are Figure 3 ; Figure 4 The multi-arm mobile robot control system and control method of the present invention are Figure 4 ; Figure 5 This is the overall control diagram of the multi-arm mobile robot control system and control method thereof of the present invention.

[0018] Explanation of the accompanying drawings in the specification: robot controller 10, joint position hard analysis unit 20, joint space point trajectory constraint model 30, comparison monitoring module 40, reinforcement learning unit 50. DETAILED DESCRIPTION

[0019] 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 the present invention and implement it. However, the embodiments are not intended to limit the present invention.

[0020] Reference Figure 1 As shown, the multi-arm mobile robot control system of the present invention includes: a robot, a robot controller 10, a joint position hard parsing unit 20 and a joint space point trajectory constraint model 30; the robot is provided with at least two robotic arms, and the robotic arms are multi-axis robotic arms, and each joint position of the multi-axis robotic arms is configured with a joint controller for driving joint movement; the robot controller 10 uses EtherCat communication to exchange data with the joint controller, and the robot controller 10 is used to send control quantities to the joint controller so that the joint controller executes the operation of the joint motor according to the control quantities; the joint position hard parsing unit 20 is a special FPGA. / ASIC is a hardware computing unit composed of the main processing core. The input of the joint position hard analysis unit 20 is the reading of the encoder at the joint and the DH parameter at this joint. Then the joint position hard analysis unit 20 is based on the forward kinematics model of the robotic arm, and calculates the coordinate position of each joint or robotic arm in the spatial coordinate system to output the result; the joint space point trajectory constraint model 30 receives the output result of the joint position hard analysis unit 20 as input, and the joint space point trajectory constraint model 30 is used to form the correction amount of the joint and superimpose it on the original control amount of this joint to become the final control amount for controlling this joint. The final control amount is used to output to the joint controller to realize joint movement.

[0021] On the basis of the above solution, 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. Figure 2As shown, the comparison monitoring module 40 is used to compare the control variables of the robot controller 10 and the output results of the joint position hard parsing unit 20 for the same joint. Specifically, the "position coordinate data of all joint axes" obtained by the joint position hard parsing unit 20 and the "position coordinates of all joint axes" obtained by the robot controller 10 are different because the former coordinate data is calculated by the joint position hard parsing unit 20, while the latter is calculated by the robot controller 10 based on a program software algorithm. In theory, the two calculation results should be consistent. However, in the present invention, the joint position hard parsing unit 20 uses hardware-based parallel computing, which has higher real-time performance. Therefore, the calculation results of the joint position hard parsing unit 20 and 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 the certain range, it indicates a problem with a certain joint, and an alarm can be sent to the robot controller 10. Therefore, the comparison monitoring module 40 here ensures that the calculated joint spatial position coordinates are more reliable and more real-time.

[0022] On the basis of the above scheme, the spatial movement of each joint in the joint space point trajectory constraint model 30 is subject to two constraints: one is the spatial constraint of the trajectory planning path of the robotic arm in 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 movement of any joint will generate a collision avoidance space with other joints (joints located on other arms that are adjacent to it in spatial position) to avoid the possibility of collision.

[0023] Among them, the position coordinates of a joint space also include the physical size information of the joint. For the sake of clarity and simplicity, the position coordinates of a joint are described by the coordinate data of the center of the joint axis. Therefore, the constraints of the point trajectory constraint model (30) for any joint in the joint space include: ; ; ; ; in, is the velocity of the jth joint of the i-th robotic arm; is the joint speed limit; is the acceleration of the jth joint of the i-th robotic arm; is the joint acceleration limit; is the angular acceleration of the jth joint of the i-th robotic arm; is the joint angular acceleration limit; is the angular velocity of the jth joint of the i-th robotic arm; is the joint angular velocity limit.

[0024] is the position coordinate of the jth joint of the i-th robotic arm in the unified coordinate system at the k-th moment.

[0025] The collision constraints between two joints of different manipulators on the robot are: ; ; ; in, is the i-th joint of the α-th manipulator at the K-th moment; is the jth joint of the βth manipulator at the Kth moment, β≠α; is the Euclidean minimum distance in the xy,yz,xz two-dimensional plane.

[0026] Furthermore, the constraints between the joint variations of different manipulator arms on the robot are: ; ; ; Furthermore, the constraint condition is set as follows: within a certain time period t1-t2, the sum of the changes in the Euclidean distance must be greater than the set value , which is the minimum value allowed in the xy plane; the same applies to the yz plane and the xz plane.

[0027] ; ; ; in, Set the minimum Euclidean distance between two joints of different manipulators in the xy plane; Set the minimum Euclidean distance between two joints of different manipulators in the yz plane; Set the minimum Euclidean distance between two joints of different manipulators in the xz plane; During this time period, if the sum of the changes in the 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 lower than a certain value, it means that the two joints tend to be at a dangerous distance in the direction of movement.

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

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

[0030] like Figure 4 As shown in the figure, the final control variable for the robot's motion control trajectory, or the "final command," directly corrects the position of one of the two closest joints on different robotic arms in real time to prevent collision. This can be considered a function similar to "elastic mutual exclusion," preventing collisions between joints of any two robotic arms that are spatially adjacent.

[0031] When the continuous Euclidean distances on the xy, yz, and xz planes are minimized and the change in Euclidean distance on the corresponding plane approaches or reaches a certain negative range, when this condition is triggered, the correction value for the control variable of the i-th joint on a certain plane is calculated and superimposed on the original control variable to become the final control variable output (final instruction). At the same time, this information is returned to the robot controller 10 to eliminate or compensate for any problems caused by directly correcting a certain control instruction. The general rule is to correct the control variable of a joint with a higher movement speed, but other rules can also be set. Figure 4 middle, is the control value of joint i on the α arm, is the adjustment value of the control amount of joint i on the α arm; + The final control quantity of the robot motion control trajectory is analyzed to form the final total system as follows: Figure 5 shown.

[0032] Based on the above system, a control method of a multi-arm mobile robot control system includes the following steps: S1, the robot controller generates the motion control trajectory instructions of the robot 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) of each joint to be reached in step S1 into the joint position hard parsing unit. The joint position hard parsing unit performs spatial position calibration and mapping on the spatial position coordinates (x, y, z) to form spatial matrix data that can represent the relationship between the spatial positions of each joint; S3, outputting the spatial matrix data to the joint space point trajectory constraint model, correcting the input data through the constraint relationship in the joint space point trajectory constraint model, and outputting the corrected final control amount to the joint controller; S4. The joint controller drives the movement of each joint according to the final control amount.

[0033] In step S3, when the continuous Euclidean distances on the three planes in the (x, y, z) coordinate system are minimized and the change in the Euclidean distance on the corresponding plane is close to or has reached a certain negative range, when this condition is met and 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.

[0034] Specifically, consider a four-arm mobile robot, each with six axes, for a total of 24 axes. Each servo joint is equipped with a servo joint motor driver, also known as a joint controller, for a total of 24 joint controllers. The robot controller 10 generates motion control trajectory commands for the robot arm, namely the spatial position coordinates (x, y, z) to be achieved by each joint. The four arms have a total of 24 sets of position coordinates. The spatial positions of the 24 joints are used as 24 controllable point targets, which serve as input to the joint position hard parsing unit 20. The joint position hard parsing unit 20 spatially calibrates and maps this multi-point position data, generating spatial matrix data that represents the spatial relationships between the positions. This matrix data is then output to the joint spatial point trajectory constraint model 30, which establishes a constraint model that includes the physical dimensions of the relevant joint points and their spatial relationships. Under the constraints of this model, the original "starting command" input is modified, and the modified "final command" is output to the joint controller.

[0035] Specifically, the joint spatial point trajectory constraint model 30 defines the spatial positional interferometry of 24 axes and the positional boundaries that encompass the physical dimensions of the joints. Using the overall spatial operating range as a constraint and the robot arm's kinematics and dynamics as a foundation, it controls the adjustment of each joint in real time to avoid collisions and deadlocks.

[0036] Therefore, the present invention proposes a four-arm coordinated control system to achieve four-arm coordinated control, avoid collision, avoid deadlock, etc.

[0037] Obviously, the above embodiments are merely examples for clarity of explanation and are not intended to limit the implementation methods. Those skilled in the art will appreciate that other variations or modifications can be made based on the above description. It is not necessary and impossible to enumerate all implementation methods here. Obvious variations or modifications arising therefrom remain within the scope of protection of the present invention.

Claims

1. A multi-arm mobile robot control system, characterized in that: include: A robot having at least two robotic arms, wherein the robotic arms are multi-axis robotic arms, and each joint position of the multi-axis robotic arms is configured with a joint controller for driving joint movement; A robot controller that communicates and exchanges data with the joint controller, wherein the robot controller is used to send a control value to the joint controller so that the joint controller can execute the operation of the joint motor according to the control value; The joint position hard analysis 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 space point trajectory constraint model receives the output result of the joint position hard analysis unit as input. The joint space point trajectory constraint model is used to form the correction amount of the joint and superimposed on the original control amount of this joint to become the final control amount for controlling this joint. The final control amount is used to output to the joint controller to realize joint movement.

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

3. The multi-arm mobile robot control system according to claim 1 or 2, characterized in that: The control system also includes a reinforcement learning unit, which receives and optimizes the final control quantity of the joint space point trajectory constraint model 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 of the joint position hard analysis 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, is the velocity of the jth joint of the i-th robotic arm; is the joint speed limit; is the acceleration of the jth joint of the i-th robotic arm; is the joint acceleration limit; is the angular acceleration of the jth joint of the i-th robotic arm; is the joint angular acceleration limit; is the angular velocity of the jth joint of the i-th robotic arm; is the joint angular velocity limit.

6. The multi-arm mobile robot control system according to claim 5, characterized in that: The collision constraint conditions between two joints of different manipulator arms on the robot are: ； ； ； in, is the i-th joint of the α-th manipulator at the K-th moment; is the jth joint of the βth manipulator at the Kth moment, β≠α; is the Euclidean minimum distance in the xy,yz,xz two-dimensional plane.

7. The multi-arm mobile robot control system according to claim 6, characterized in that: The constraint conditions between the joint variations of different manipulator arms on the robot are: ； ； ； Furthermore, the constraint conditions are set as follows: within a certain time period t1-t2, ; ; ; in, Set the minimum Euclidean distance between two joints of different manipulators in the xy plane; Set the minimum Euclidean distance between two joints of different manipulators in the yz plane; Set the minimum Euclidean distance between two joints of different manipulators in the xz plane.

8. The control method of a multi-arm mobile robot control system according to any one of claims 1 to 7, characterized in that: The steps include: S1, the robot controller generates the motion control trajectory instructions of the robot 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) of each joint to be reached in step S1 into the joint position hard parsing unit. The joint position hard parsing unit performs spatial position calibration and mapping on the spatial position coordinates (x, y, z) to form spatial matrix data that can represent the relationship between the spatial positions of each joint; S3, outputting the spatial matrix data to the joint space point trajectory constraint model, correcting the input data through the constraint relationship in the joint space point trajectory constraint model, and outputting the corrected final control amount to the joint controller; S4. The joint controller drives the movement of each joint according to the final control amount.

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

10. The control method of the multi-arm mobile robot control system according to claim 9, characterized in that: In step S3, when the continuous Euclidean distances on the three planes in the (x, y, z) coordinate system are minimized, and the change in the Euclidean distance on the corresponding plane is close to or has reached a certain negative range, when this condition is met and 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.

Images