Robot system control method, robot controller, and storage medium

Abstract

The embodiment of the application provides a robot system control method, a robot controller and a storage medium, and belongs to the technical field of robot control; the method is applied to a first controller, and the method comprises the following steps: constructing a plurality of real-time tasks and a plurality of non-real-time tasks with different priorities, the plurality of non-real-time tasks comprise a plurality of instance tasks, each instance task is correspondingly arranged with a second controller, and the priority of the real-time task is higher than that of the non-real-time task; each second controller is in communication connection with the first controller; in response to receiving a preset interrupt, each real-time task and instance task is round-robin scheduled according to the priority of each real-time task and non-real-time task, wherein each instance task is scheduled after a corresponding business operation of each real-time task is completed, each real-time task is activated by acquiring a corresponding semaphore to complete a corresponding business operation, and each semaphore is released in turn according to the corresponding priority. The embodiment of the application can improve the real-time performance of the master robot in managing the slave robots.

Description

Technical Field

[0001] This application relates to the field of robot control technology, and in particular to a robot system control method, a robot controller, and a storage medium. Background Technology

[0002] Robotic systems typically consist of multiple robots working together under coordinated control. In practical applications, to ensure that coordinated control meets control requirements, a master robot is usually selected as the master robot, and the others act as slave robots, forming a master-slave architecture. In this master-slave architecture, the master robot usually controls the slave robots, thereby reducing the probability of mutual interference between robots during coordinated control. However, in practical applications, it is difficult to ensure the real-time management of the slave robots by the master robot in a master-slave architecture. Summary of the Invention

[0003] The main objective of this application is to provide a robot system control method, robot controller, and storage medium that can improve the real-time management of slave robots by the master robot.

[0004] To achieve the above objectives, a first aspect of this application provides a robot system control method, wherein the robot system control method is applied to a first controller, and the method includes: Multiple real-time tasks and multiple non-real-time tasks with different priorities are constructed. The multiple non-real-time tasks include multiple instance tasks. Each instance task is set to correspond to a second controller. The priority of the real-time tasks is higher than that of the non-real-time tasks. Each second controller is communicatively connected to the first controller. In response to receiving a preset interrupt, the real-time tasks and instance tasks are scheduled in a round-robin fashion according to the priorities of the real-time tasks and the non-real-time tasks. Each instance task is scheduled after the real-time task completes its corresponding business operation. Each real-time task is activated by acquiring its corresponding semaphore to complete its corresponding business operation. Each semaphore is released sequentially according to its corresponding priority.

[0005] To achieve the above objectives, a second aspect of the present application provides a robot controller, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the robot system control method as described in any one of the first aspects.

[0006] To achieve the above objectives, a third aspect of the present application provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the robot system control method described in any of the first aspects.

[0007] The robot system control method, robot controller, and storage medium proposed in this application divide tasks into real-time tasks and non-real-time tasks, assign priorities to each real-time task and non-real-time task, and strictly control the execution order of each real-time task and the execution order between each real-time task and non-real-time task based on the semaphores corresponding to each real-time task and their priorities during the execution of each task. At this time, a real-time streaming management architecture is formed on the first controller. This real-time streaming structure ensures that the first controller, as the master controller, can effectively manage multiple second controllers, as slave controllers, in real time, improving the real-time performance of the master controller's management of the slave controllers. Compared with related technologies, the embodiments of this application improve the real-time performance of the master robot's management of each slave robot. Attached Figure Description

[0008] Figure 1 This is a flowchart illustrating an embodiment of the robot system control method provided in this application; Figure 2 This is a schematic diagram of the coordinate systems of the master and slave robotic arms satisfying tight cooperative constraints in one embodiment of the robot system control method provided in this application. Figure 3 This is a schematic diagram of the application process of collaborative control in one embodiment of the robot system control method provided in this application; Figure 4 This is a schematic diagram of the coordinate systems of the master and slave robotic arms satisfying the loose cooperation constraint condition in another embodiment of the robot system control method provided in this application; Figure 5 This is a schematic diagram of the state transition of the state machine in one embodiment of the robot system control method provided in this application; Figure 6 This is a task topology diagram of the first controller in one embodiment of the robot system control method provided in this application; Figure 7 This is a schematic diagram of the network topology in one embodiment of the robot system provided in this application; Figure 8 This is a schematic diagram of the hardware structure of the robot controller corresponding to the robot system control method provided in this application. Detailed Implementation

[0009] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0010] It should be noted that although functional modules are divided in the device schematic diagram and a logical order is shown in the flowchart, in some cases, the steps shown or described may be performed in a different order than the module division in the device or the order in the flowchart. The terms "first," "second," etc., in the specification, claims, and the aforementioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence.

[0011] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used herein is for the purpose of describing embodiments of this application only and is not intended to limit this application.

[0012] Robotic systems typically consist of multiple robots working together in a collaborative control system. In practical applications, to ensure that collaborative control meets control requirements, a master robot is usually selected as the master robot, and the remaining robots are slave robots, forming a master-slave architecture. In this master-slave architecture, the master robot usually controls the slave robots, thereby reducing the probability of mutual interference between robots during collaborative control. However, in practical applications, collaborative robot control not only requires motion control for each robot but also management of the collaborative states between them. Therefore, as the complexity of collaborative control requirements increases, the number of slave robots managed by the same master robot also increases, posing a severe challenge to the real-time processing capability of the master robot. While real-time requirements can be met by expanding the robot's hardware in practical applications, the corresponding hardware costs also increase. Therefore, in practical applications, it is difficult for a master-slave architecture-based robotic system to ensure the real-time management of each slave robot by the master robot. Based on this, embodiments of this application provide a robot system control method, a robot controller, and a storage medium that can improve the real-time management of each slave robot by the master robot.

[0013] Reference Figure 1 As shown, according to the robot system control method provided in the embodiments of this application, the robot system control method is applied to a first controller, and the method includes: Step S100: Construct multiple real-time tasks and multiple non-real-time tasks with different priorities. The multiple non-real-time tasks include multiple instance tasks. Each instance task is set to correspond to a second controller. The priority of real-time tasks is higher than that of non-real-time tasks. Each second controller is connected to the first controller. Step S200: In response to receiving a preset interrupt, the real-time tasks and instance tasks are scheduled in a round-robin fashion according to the priority of each real-time task and non-real-time task. Each instance task is scheduled after the real-time task completes the corresponding business operation. Each real-time task is activated by acquiring its corresponding semaphore to complete the corresponding business operation. Each semaphore is released in sequence according to its corresponding priority.

[0014] Therefore, by dividing tasks into real-time and non-real-time tasks, and assigning priorities to each real-time and non-real-time task, and strictly controlling the execution order of each real-time task and the execution order between each real-time and non-real-time task during task execution based on the semaphores corresponding to each real-time task and their priorities, a real-time streaming management architecture is formed on the first controller. This real-time streaming structure ensures that the first controller, as the master controller, can effectively manage multiple second controllers, as slave controllers, in real time, improving the real-time performance of the master controller's management of the slave controllers. The embodiments of this application improve the real-time performance of the master robot's management of each slave robot.

[0015] The priorities in this application's embodiments can be pre-configured, such as configuration file loading or program writing. This application's embodiments will not elaborate on these points.

[0016] A preset interrupt indicates that the interrupt triggering method is pre-configured. For example, a preset interrupt is triggered by the system crystal oscillator. This application embodiment does not limit what triggers the interrupt, nor does it limit the frequency period of the interrupt triggering. For example, in some embodiments, the interrupt is a 1kHz crystal oscillator interrupt. This application embodiment will not elaborate on these specific instances.

[0017] Semaphores correspond one-to-one with real-time tasks. Non-real-time tasks are invoked only after all real-time tasks have been executed. For example, the scheduler can be started to schedule non-real-time tasks before the semaphore of the last real-time task is released. Therefore, in some embodiments, step S200 of this application embodiment can also be understood as responding to the received interrupt by releasing the corresponding semaphores in sequence according to the priority of each real-time task, so that each real-time task completes the business operation of the current cycle in sequence according to its priority; and after each real-time task completes the business operation of the current cycle, the instance tasks are scheduled in a round-robin manner according to the priority of each non-real-time task.

[0018] Real-time tasks are used for processing procedures in robot control that require high real-time performance and precise cycle intervals. Different real-time tasks have different processing flows. This application does not limit the number of real-time tasks. Non-real-time tasks include at least instance tasks, which can be of various types, each corresponding to a second controller. Non-real-time tasks are used to process data for which the robot's real-time requirements are not high. In some embodiments, non-real-time tasks also include non-instance tasks, each of which manages all of the robot's non-real-time data.

[0019] Steps S100 to S200 can be executed when the first controller acts as the master controller. The first controller can be determined to be the master controller through negotiation, or it can be determined directly by manual configuration or based on physical networking. In this application embodiment, there is no restriction on under what circumstances the first controller is confirmed to be the master controller for steps S100 to S200. Those skilled in the art can selectively set them according to the actual situation.

[0020] When the second controller acts as the main controller, the relevant steps of the first controller can be followed. This application will not elaborate on these steps.

[0021] This application does not limit the number of second controllers. Each second controller is associated with a robot body, and the robot body includes at least one robotic arm. When multiple robotic arms are provided, one of the robotic arms can be the master robotic arm, and the rest can be slave robotic arms.

[0022] The construction of real-time tasks and non-real-time tasks in step S100 can be carried out simultaneously or separately at different time periods. For example, non-real-time tasks are created when adding robot instance configuration information for the second controller, while real-time tasks are created during initialization. This application embodiment does not impose any restrictions on this.

[0023] Understandably, the multiple real-time tasks include EtherCAT communication tasks, User Datagram Protocol (UDP) communication tasks, and real-time motion control tasks; the scheduling of each real-time task includes the following steps: In response to receiving a preset interrupt, the first semaphore is released to the EtherCAT communication task to activate the EtherCAT communication task. Before the EtherCAT communication task completes the interactive processing of the control synchronization data for the current cycle and enters the blocking state, the second semaphore is released to the UDP communication task to activate the UDP communication task; the interactive processing of control synchronization data includes updating the real-time pose data of the second controller to the preset data storage space. After the UDP communication task sends the second path planning result data of the previous cycle to the corresponding second controller and before it enters the blocking state, the third semaphore is released to the real-time motion control task to activate the real-time motion control task. The real-time motion control task performs real-time trajectory planning on the real-time pose data stored in the data storage space in the current cycle, and stores the first planning result data into the data storage space. The path planning result data is determined based on the planning processing results.

[0024] EtherCAT communication tasks are tasks based on the EtherCAT protocol. They use the EtherCAT protocol to communicate with each second controller. Compared with direct physical connection, this method can reduce hardware requirements while ensuring real-time performance.

[0025] Control synchronization data represents hard real-time data. It can include real-time data collected in the current cycle or control data calculated in the previous cycle, thus dividing the acquisition and control into different cycle management and improving the real-time performance of the interaction.

[0026] The EtherCAT communication task is used to ensure clock synchronization between the first and second controllers and to provide robot-related data for computation to other low-priority real-time tasks. In some embodiments, the EtherCAT communication task is also used to send generator control parameters to the second controller. Exemplarily, in some embodiments, the control synchronization data includes motor control parameters from the previous cycle, clock synchronization parameters for the current cycle, and real-time pose data for the current cycle, wherein, as... Figure 6 As shown, the real-time pose data includes the motion parameters of the servo motor and the sensing data of the force sensor.

[0027] Real-time trajectory planning processing involves interpolating trajectory points based on a planned trajectory. This application does not limit the trajectory point interpolation method; those skilled in the art can selectively set it according to actual conditions. For example, in some embodiments, trajectory point planning is based on position trajectory interpolation, and the real-time pose data includes collected displacement, velocity, acceleration, and other real-time pose data. In other embodiments, trajectory point planning is trajectory interpolation in Cartesian space, and the real-time pose data includes Cartesian pose information.

[0028] A UDP communication task is a task that uses the UDP protocol for communication.

[0029] The second path planning result data is used to indicate the motor-related parameters of the trajectory point that the corresponding second controller expects to reach in the current cycle. In some embodiments, trajectory interpolation is entirely completed by the real-time motion control task; therefore, for the current cycle t, the second path planning result data of the previous cycle t-1 represents the first path planning result data calculated based on the real-time motion control task in cycle t-1. In other embodiments, trajectory planning is divided into real-time motion control tasks and other non-real-time tasks; in this case, the second path planning result data is obtained by processing the first planning result data of the real-time motion control task based on the non-real-time tasks in the previous cycle. This application will not elaborate further on these aspects in its embodiments.

[0030] Therefore, by separating the distribution of control parameters for robot motion and trajectory point interpolation into tasks with different cycles and priorities, and separating trajectory point interpolation and the real-time pose data required for interpolation into tasks with different priorities but within the same cycle, the processing cycle for tasks requiring high real-time performance is shortened. Within the same cycle, the second controller is notified first to execute motion control before trajectory point interpolation, thus ensuring more real-time control of each second controller even with multiple controllers. Furthermore, the motion control-related parameters of the second controller are executed by the two highest-priority tasks: EtherCAT communication and UDP communication, further ensuring the real-time effectiveness of the second controller.

[0031] Setting the UDP communication task to be lower than the EtherCAT communication task and higher than the real-time motion control task can further reduce the impact of the long real-time motion control task on the real-time performance of UDP communication, while also ensuring that the UDP communication cycle meets the jitter requirements. This reduces the probability of startup delay when the slave robot follows the master robot's movements in the second controller.

[0032] Understandably, the multiple real-time tasks include real-time motion control tasks and real-time communication tasks; the real-time communication tasks include at least one of EtherCAT communication tasks and UDP communication tasks; the multiple instance tasks include cooperative control instance tasks, each of which performs the following steps: The first end pose information of the first target controller corresponding to the collaborative control instance task is obtained from the first path planning result data stored in the preset data storage space. The path planning result data is obtained by real-time trajectory planning processing of the real-time pose data of the main robotic arm of each second controller in the current cycle of the real-time motion control task. Obtain the cooperative pose constraint information of the first target controller in the current cycle; Based on the first end-effector pose information and the cooperative pose constraint information, the second end-effector pose information corresponding one-to-one with the slave robot arm of the first target controller is obtained. The second path planning result data of the first target controller is obtained based on the first end pose information and the second end pose information of the same first target controller and updated to the data storage space, so as to notify the first target controller of the second path planning result data through a real-time communication task.

[0033] The first target controller is one of multiple second controllers. Assuming that the collaborative control instance task is set to correspond one-to-one with the second controller, and the currently scheduled collaborative control instance task is collaborative control task 1, then when collaborative control task 1 is executed, the first target controller is the second controller corresponding to collaborative control task 1.

[0034] The collaborative pose constraint information and the slave robotic arms that move with the master robotic arm in the first target controller are set to correspond one-to-one.

[0035] In some embodiments, the collaborative control instance task can be set to correspond one-to-one with the slave robot of the first target controller, or it can be set to correspond one-to-one with the first target controller.

[0036] The collaborative control instance task is used by the corresponding first target controller to perform synchronous control of the master robot arm and the slave robot arm. Since the position command of the master robot arm has not changed, the position command of the slave robot arm of the same second controller will not change either, and there is no need to obtain the feedback information of the slave robot arm in real time. Therefore, the collaborative control task can be set as a high-priority non-real-time task, so as to meet the planning timeliness requirements without affecting the real-time performance of the real-time task.

[0037] In this embodiment, trajectory interpolation is divided into two different tasks. The pose planning of the robotic arm is performed by the collaborative control instance task. At this time, the real-time motion control task only needs to perform trajectory point planning for the main robotic arm of the second controller, thereby making the planning efficiency of collaborative control faster.

[0038] This application embodiment does not impose restrictions on the constraints on the execution of collaborative control instance tasks in the polling time slice. The tasks can be executed by default in the corresponding polling time slice, or they can be executed after the preset start conditions are met. This application embodiment can selectively set these constraints according to actual business needs and the state of the first target controller.

[0039] In some embodiments, the real-time communication task includes an EtherCAT communication task, meaning the real-time task comprises an EtherCAT communication task and a real-time motion control task. In this case, the second path planning result data from the previous cycle is sent by the EtherCAT communication task. In other embodiments, multiple EtherCAT communication tasks are configured. In this case, one EtherCAT communication task is used to control the interactive processing of synchronization data, and another EtherCAT communication task is used to send the second path planning result data for the migration cycle. In still other embodiments, the real-time communication task includes an EtherCAT communication task and a UDP communication task. In this case, the real-time task comprises an EtherCAT communication task, a UDP communication task, and a real-time motion control task.

[0040] This application does not restrict whether the cooperative pose constraint information is calculated in real time in the cooperative control instance task or calculated in other tasks. Those skilled in the art can selectively set it according to the actual situation.

[0041] The embodiments of this application do not limit the form of the data storage space, and those skilled in the art can selectively set it according to the performance of data reading and writing and the data capacity.

[0042] This application does not limit how the second path planning result data is obtained. In some embodiments, the second path planning result data can be calculated by determining whether tight and loose cooperative constraints are satisfied. Whether tight and loose cooperative constraints are satisfied can be determined by the real-time motion control task.

[0043] For example, the following example shows a first target controller with a master robotic arm and a slave robotic arm, referring to the present application. Figure 2 , Figure 3 , Figure 4 The robot system control method according to embodiments of this application is described in detail below: like Figure 2 As shown, when the target object is clamped and fixed by the master and slave robotic arms, the tight cooperation constraint condition is satisfied. At this time, the positional relationship between the master and slave robotic arms is relatively fixed, and the coordinate system of each position is as follows: Figure 2 As shown, where, Indicates the world coordinate system. This represents the free coordinate system from the end effector of the robotic arm. This indicates the coordinate system from which the base of the robotic arm is located (i.e., the second base coordinate system in the above embodiment). This represents the free coordinate system where the end effector of the main robotic arm is located. The coordinate system representing the base of the main robotic arm (i.e., the first base coordinate system in the above embodiment) is used. When the main robotic arm and the slave robotic arm release the target object and satisfy the tight collaboration constraint condition, the positional relationship between the end effector of the main robotic arm and the end effector of the slave robotic arm can also be maintained consistent with the positional information under the tight collaboration constraint condition. Therefore, the second end effector pose information can be obtained in real time based on the collaborative pose constraint information, which can realize the movement of the slave robotic arm following the main robotic arm under tight collaboration and loose collaboration conditions. Taking the collaborative control instance task polled at the current moment as task 1 and the second controller corresponding to task 1 as controller 1 as an example, the synchronization control steps of task 1 are as follows. Figure 3 As shown: S1. Read the cooperative pose constraint information stored in the history of the first communication instance task from the data storage space: 1) In some embodiments, the cooperative pose constraint information can be calculated by the first communication instance task using the following first constraint matrix formula: (Formula 2); in, , All of these are data points from the first Cartesian pose data. , All of these are data from the second Cartesian pose data; This represents the first pose acquisition information, that is, the pose information of the first base coordinate system of the main robotic arm in the world coordinate system within the current cycle; This represents the second pose acquisition information, which is the pose information of the end effector of the main robotic arm in the first base coordinate system during the current cycle; This represents the third pose acquisition information, which is the pose information of the robot arm from the second base coordinate system in the world coordinate system during the previous cycle; This represents the fourth pose acquisition information, which is the pose information from the end of the robotic arm in the second base coordinate system during the current cycle.

[0044] At this point, the solution obtained by the real-time motion control task within the current cycle is based on the positive kinematics transformation matrix. Substituting these values ​​into the formula for the first constraint matrix yields the cooperative pose constraint information for the current cycle.

[0045] The principle for obtaining the cooperative pose constraint information in Formula 2 is as follows: For closely coordinated tasks, since the two robotic arms need to grip an object during cooperation, there is no relative displacement between their ends and the object, and the relative velocity should be zero. Therefore, it is necessary to add positional constraints on the ends of the two robotic arms. Assume there is an object coordinate system. This object coordinate system can correspond to the coordinate system of the real object being transported, or it can correspond to a virtual object (that is, the target object in the above embodiment can be a real object or a virtual object). As a description of the relative position constraints of the two robotic arm ends, then... Figure 2 The coordinate systems shown satisfy the following formula three: (Formula 3); in, The transformation matrix represents the transformation from the world coordinate system to the centroid coordinate system where the centroid of the target object is located, which is also the pose information of the centroid coordinate system in the world coordinate system; , This represents the transformation matrix from the world coordinate system to the left and right arm base coordinate systems, respectively. , This represents the transformation matrix that transforms the base coordinate system of the master and slave robotic arms to their respective end-effector free coordinate systems; , This represents the transformation matrix that transforms the end-effector coordinate system of the master and slave robotic arms to the same centroid coordinate system.

[0046] At this point, Formula 4 can be derived from Formula 3 as follows: (Formula 4); In some embodiments, , It can be obtained through dual-arm robot model files or actual measurements during assembly; , It can be obtained by solving the robot's forward kinematics; , Depending on the object being manipulated and the task scenario, the coordinate system of the centroid of the object can be estimated using robot vision information. Relative to the world coordinate system Transformation matrix Therefore, the robot's end-effector pose when grasping an object can be used to calculate... , .

[0047] At this point, based on Formula 4, in , In the case of the unknown, we can obtain: ; That is to say .

[0048] 2) In other embodiments, the cooperative pose constraint information is calculated using the following second constraint matrix formula: (Formula 3); in, This matrix represents the information acquired in the fifth pose, and is one of the data points in the first Cartesian pose data. The matrix corresponding to the information acquired in the sixth pose (that is, the pose information of the target object's centroid coordinate system in the free coordinate system at the end of the robotic arm) is one of the data in the second Cartesian pose data; The matrix representing the inverse pose information of the sixth pose acquisition information (that is, the pose information of the target object's centroid coordinate system in the end free coordinate system of the main robotic arm).

[0049] in, , It can be directly measured, and once measured, it can be substituted into Formula 3 to calculate the cooperative pose constraint information.

[0050] This application does not limit which constraint formula is used to calculate the cooperative pose constraint information. Those skilled in the art can selectively set the formula according to actual needs. For example, if the accuracy of the target object's coordinate acquisition does not meet the conditions, such as too many interference conditions or inability to acquire the coordinates, then Formula 2 can be used. Conversely, if the accuracy of the target object's coordinate acquisition meets the preset acquisition accuracy conditions, then Formula 3 can be used. This application will not elaborate on these points further.

[0051] In other embodiments, Formula 2 and Formula 3 can be set simultaneously in the robot's controller, allowing users to selectively configure them based on the actual operating environment, thereby adapting to different production environments.

[0052] S2. If the current cycle is determined to satisfy the tight cooperative constraint condition based on the real-time motion control task, then based on the first end pose information... Combined with the cooperative pose constraint information, the second end-effector pose information is obtained. The details are as follows: Substitute the first end pose information obtained from the real-time planning of the current cycle into the following formula: ; in, This is the matrix corresponding to the pose information of the second end point; The matrix corresponding to the sixth pose acquisition information represents the pose information of the first base coordinate system in the world coordinate system; The matrix corresponding to the seventh pose acquisition information represents the pose information of the second base coordinate system in the world coordinate system.

[0053] In some embodiments, both the master robotic arm and the slave robotic arm have their own end-effector reference coordinate systems established by their respective end-effector reference objects. , When the robot's main arm completes trajectory planning and performs closely coordinated motion, the reference coordinate system of the end effector of the main robotic arm... It runs along the planned trajectory and uses cooperative pose constraint information. And Formula 1 calculates in real time from the robotic arm's reference coordinate system. The transformation matrix relative to the base coordinate system of the robotic arm is as follows: ; in, First end reference coordinate system The robot arm's pose information is obtained from the first base coordinate system. Indicates the second end reference coordinate system The robot arm's pose information is obtained from the second base coordinate system. However, in practical applications, under the condition of tight collaboration, , Since it is an identity matrix, under tight cooperative constraints, the pose information of the second end point can be directly used. As pose information from the end effector of the robotic arm, the first end effector pose information The pose information of the end effector of the main robotic arm.

[0054] S3. Based on the real-time motion control task, if the relaxed coordination constraint condition is satisfied, then according to the first end-effector pose information... Based on the cooperative pose constraint information, the third end-effector pose information of the main robotic arm is obtained, and the fourth end-effector pose information of the robotic arm is obtained; specifically as follows: Reference Figure 4 As shown, the free coordinate system at the end of the main robotic arm Around the reference coordinate system Taking motion as an example. From the perspective of the world coordinate system, the reference coordinate system... The origin It moves along the planned trajectory; however, from the perspective of the free coordinate system at the end of the main robotic arm, the reference coordinate system... The origin It is stationary. Then, as... Figure 4 The lighter-colored master and slave robotic arms are shown in the diagram. The free coordinate system at the end of the master robotic arm is also shown. Relative to the reference coordinate system The transformation matrix is Similarly, the situation is similar from the end of the robotic arm, where there is a free coordinate system at the end of the robotic arm. Relative to the reference coordinate system The transformation matrix is At this point, the transformation matrices of the robot's master and slave arms relative to their respective base coordinate systems are: ; That is, the pose information of the third end effector. Fourth end-effector pose information .in, , , , , All of these are known quantities.

[0055] It is understood that in some embodiments, the first end reference coordinate system may coincide with the free coordinate system corresponding to the end of the main robotic arm; in other embodiments, the second end reference coordinate system may coincide with the free coordinate system corresponding to the end of the slave robotic arm; and in still other embodiments, the world coordinate system may coincide with the base coordinate system of the main robotic arm. In this application, the embodiments will not elaborate further on these points. Those skilled in the art may choose to have some coordinate systems coincide or not coincide at all. In this regard, the embodiments of this application do not impose any restrictions.

[0056] Understandably, the multiple instance tasks also include a first communication instance task, which has a lower priority than the collaborative control instance task; each first communication instance task performs the following steps: Acquire the first Cartesian pose data of the end of the main robotic arm of the second target controller corresponding to the first communication instance task, and acquire the second Cartesian pose data of the end of the robotic arm from the second target controller in real time. Based on the first and second Cartesian pose data, the cooperative pose constraint information is obtained and updated to the data storage space; Wherein, the first Cartesian pose data is used to indicate the pose information of the target object's centroid coordinate system in the end-effector coordinate system of the main manipulator during the current cycle, and the second Cartesian pose data is used to indicate the pose information of the end-effector free coordinate system of the manipulator in the centroid coordinate system of the target object during the current cycle; or, the first Cartesian pose data is used to indicate the pose information of the world coordinate system in the end-effector free coordinate system of the main manipulator during the current cycle, and the second Cartesian pose data represents the pose information of the end-effector free coordinate system of the manipulator in the world coordinate system.

[0057] The first Cartesian pose data can be acquired in real time by the EtherCAT communication task or by the first communication instance task. If the first Cartesian pose data does not need to participate in the trajectory planning of the real-time motion control task, it can be acquired in real time by the first communication instance task; otherwise, it can be read directly from the data storage space.

[0058] This application embodiment does not limit the communication protocol of the first communication instance task. For example, TCP / IP protocol or UDP protocol can be used. This application embodiment will not elaborate on these points.

[0059] For example, in some embodiments, it is assumed that the cooperative pose constraint information is represented as follows: ; but, , These are all data points from the first Cartesian pose data set, used to indicate the pose information of the world coordinate system in the end-effector free coordinate system of the main robotic arm during the current cycle. , All of these are data points from the second Cartesian pose data set, used to indicate the pose information from the end-effector's free coordinate system in the world coordinate system; among them, This represents the first pose acquisition information, that is, the pose information of the first base coordinate system of the main robotic arm in the world coordinate system within the current cycle; This represents the second pose acquisition information, which is the pose information of the end effector of the main robotic arm in the first base coordinate system during the current cycle; This represents the third pose acquisition information, which is the pose information of the robot arm from the second base coordinate system in the world coordinate system during the previous cycle; This represents the fourth pose acquisition information, which is the pose information from the end of the robotic arm in the second base coordinate system during the current cycle.

[0060] For example, in some other embodiments, the cooperative pose constraint information is assumed to be represented as follows: ; but, It is one of the data in the first Cartesian pose data, used to indicate the pose information of the target object's centroid coordinate system in the end-effector coordinate system of the main robotic arm during the current cycle. It is one of the data in the second Cartesian pose data, used to indicate the pose information of the end-effector of the robot arm in the centroid coordinate system of the target object during the current cycle.

[0061] Understandably, the first Cartesian pose data and the second Cartesian pose data of the end effector of the main robotic arm are acquired when the trajectory tracking enable state indicates that trajectory tracking is enabled and the trajectory tracking state is inactive. Each first communication instance task also performs the following steps: After obtaining the cooperative pose constraint information, the trajectory tracking state of the second target controller is activated so that each cooperative control instance task is invoked when the trajectory tracking state of the corresponding second target controller is activated.

[0062] By triggering the trajectory tracking state only after it is activated, the timing of the planning can be further ensured.

[0063] Understandably, the first controller stores a robot state machine corresponding to each second controller, and schedules real-time tasks and instance tasks in a round-robin fashion according to the priority of each real-time task and non-real-time task, including: Based on the semaphores corresponding to each real-time task, each real-time task is scheduled according to its corresponding priority order. After each real-time task is scheduled, for each instance task, within the time slice divided according to the corresponding priority, the instance task is scheduled according to the robot state of the current period determined by the corresponding robot state machine.

[0064] By controlling the start and stop of instance tasks through the robot state determined by the robot state machine, control can be simplified while ensuring real-time management of each slave robot.

[0065] This application does not limit how the state machine is set. For example, in some embodiments, the robot state machine of each second controller includes 6 states, such as... Figure 5 As shown, the six states are: idle, not connected, connected, ready, running, and stopped. The definitions of each state are as follows: The SLAVE_IDLE state indicates that the robot state machine is not associated with a real second controller, such as the robot instance information not being associated with the first controller. Robot instance information includes, for example, the IP address, port number, and base coordinate system position of the second controller. In some embodiments, the robot instance information of the robot state machine can be set via host computer commands to establish an association between the robot state machine and the corresponding second controller. In this case, refer to... Figure 5 As shown, after the robot state machine is associated with the robot instance information of a second controller, the running state of the robot state machine jumps to the unconnected state.

[0066] The "SLAVE_DISCONNECTED" state indicates that the robot state machine has established an association with the real second controller, but has not yet established a TCP / IP connection with the remote second controller through the corresponding communication instance task. In some embodiments, such as... Figure 5 As shown by the solid arrow, when the robot instance information associated with the robot's state machine in the second controller is cleared via a command from the host computer, the robot's state machine transitions back to the idle state. Figure 5 As shown by the solid arrow, after the first controller successfully establishes a TCP / IP connection with the remote second controller of the robot's state machine, the robot's state machine transitions to the connected state. In other embodiments, the first controller may also actively initiate a connection to the second controller of the robot's state machine. In some embodiments, such as... Figure 5As shown by the dashed arrow, when the communication between the first controller and the second controller meets the abnormal conditions, the robot's state machine switches to the disconnected state.

[0067] The "SLAVE_CONNECTED" state indicates that the first controller has established a stable TCP connection with the second controller corresponding to the robot instance information associated with this state machine, but the trajectory tracking function of the second controller has not yet been activated. Upon entering this state, the second controller will activate its corresponding TCP communication instance task. This TCP communication instance task will continuously maintain a TCP heartbeat with the second controller to ensure a normal TCP connection. The TCP communication instance task can also serve as the first communication instance task to calculate cooperative pose constraint information in real time. For example, when attempting to initiate the coordinated motion of the master and slave robotic arms via a host computer command, the first controller first communicates with the second controller through the TCP communication instance task to obtain the Cartesian pose of the slave robotic arm's end effector (i.e., the second Cartesian pose data). Combining this with the current Cartesian pose of the master robotic arm's end effector (i.e., the first Cartesian pose data), it calculates the coordinated pose constraint information and stores it in the data storage space corresponding to the second controller. Then, the first controller attempts to activate the second controller's trajectory tracking function. When both steps are successfully completed (i.e., the trajectory tracking function is successfully activated and the coordinated pose constraint information is updated in real time), as... Figure 5 As indicated by the solid arrow, the robot state machine of the second controller transitions to the ready state; as shown by the solid arrow. Figure 5 As indicated by the solid arrow, if both of the above steps are not completed, it means that TCP communication may be interrupted, and the robot state machine jumps to the disconnected state. In some embodiments, when the TCP connection between the first controller and the second controller is closed by an instruction, then as follows... Figure 5 As indicated by the dashed arrow, the robot's state machine will also revert to the unconnected state.

[0068] The SLAVE_READY state indicates that the second controller, corresponding to the robot instance information associated with the first controller, has activated its trajectory tracking function, but has not yet begun cooperative motion. At this time, the first controller can identify that the second controller has started trajectory tracking through the SLAVE_READY state, thereby quickly sending the pose information of the planned trajectory points to the first controller. In some embodiments, when a UDP communication task is set to send the second path planning result, the SLAVE_READY state can constrain the timing of the UDP communication task sending the second path planning result. In some embodiments, the second path planning result sent by the first controller takes effect after the start motion command has been issued. At this time, such as... Figure 5As indicated by the solid arrow, the robot state machine transitions to the running state. In some embodiments, such as... Figure 5 As indicated by the solid arrow, when the cooperative motion control of the second controller is canceled, the robot state machine reverts to the connected state. If the cooperative motion control of the second controller is canceled and a TCP communication error occurs (such as an interruption, or the first controller failing to forward the start-of-motion command to the second controller), then... Figure 5 As indicated by the dashed arrow, the robot's state machine transitions to the unconnected state.

[0069] The SLAVE_RUNNING state indicates that the second controller, corresponding to the robot instance information associated with the first controller, has begun cooperative motion. In some embodiments, such as... Figure 5 As shown by the solid arrow, when the first controller determines that the second controller has stopped cooperative control (e.g., upon receiving a command to cancel cooperative control), the robot's state machine transitions to the stopped state. If the second controller issues a command to stop cooperative control and a TCP communication anomaly occurs (e.g., a heartbeat interruption, or no return data received after sending three consecutive UDP data packets, or the first controller is unable to forward the command to stop cooperative motion to the second controller), then... Figure 5 As indicated by the dashed arrow, the robot's state machine transitions to an unconnected state.

[0070] The SLAVE_STOPPING state indicates that the second controller, corresponding to the robot instance information associated with the first controller's state machine, is ceasing cooperative motion. By setting the SLAVE_STOPPING state, the first controller can stop sending cooperative motion trajectory points to the second controller when trajectory tracking is deactivated. This reduces the probability of receiving trajectory point parameters during the 10-30ms timeframe required for the second controller to deactivate trajectory tracking after the robot arm stops moving. In the SLAVE_STOPPING state, the first controller automatically sends commands to the second controller via TCP communication to stop motion and disable trajectory tracking. Upon receiving confirmation from the second controller that cooperative motion has ceased, the robot's state machine switches to the connected state, awaiting further operations. If the first controller fails to receive confirmation from the second controller, the communication connection between them is considered broken, and the robot's state machine transitions to the disconnected state.

[0071] Understandably, the data storage space includes data instance storage spaces that correspond one-to-one with the second controller. Each data instance storage space is used to store instance data corresponding to the second controller. The instance data includes at least one of real-time pose data, first planning processing result data, second planning processing result data, and cooperative pose constraint information.

[0072] By setting up data instance storage space, the instantiation management of each second controller can be achieved.

[0073] Understandably, non-real-time tasks also include anomaly detection tasks. The priority of anomaly detection tasks is lower than that of the first communication instance task. Anomaly detection tasks perform the following steps in the time slice allocated by the corresponding priority. Capture error controller exception information reported from real-time tasks and / or instance tasks, and obtain the capture results; If, based on the capture results, it is determined that there is at least one abnormal data from a third target controller, the motion parameters of the third target controller in the previous cycle are recorded as the last time in the normal state and saved in the preset data storage space. Abnormal data when interacting with external devices.

[0074] By setting up independent anomaly detection tasks, unified management and control of anomalies across all secondary controllers can be achieved.

[0075] For example, see below. Figure 6 A schematic diagram illustrating the software topology of the first controller applying the above-described control method according to an embodiment of this application is shown below. Figure 6As shown, the first controller has multiple tasks arranged in descending order of priority: EtherCAT communication task, UDP communication task, real-time motion control task, cooperative control instance task, trajectory planning task, instruction parsing task, first communication instance task, and anomaly detection task. The priority of the trajectory planning task, instruction parsing task, and anomaly detection task can be adjusted to be lower than that of the first communication instance task. The EtherCAT communication task, UDP communication task, and real-time motion control task are real-time tasks, while the cooperative control instance task and the first communication instance task are instance tasks corresponding one-to-one with the second controller. In some embodiments, the first controller is configured with a data storage instance space corresponding one-to-one with the second controller; in other embodiments, the first controller is configured with a data storage instance space corresponding one-to-one with the robotic arm. This allows each data storage instance space to handle the robot-related data required for different robotic arm motion control, ensuring that relevant data can flow and synchronize between multiple robot tasks. Robot-related data includes joint position feedback data, joint position command data, pose of the base coordinates relative to the world coordinate system, cooperative motion constraints of the master and slave robotic arms, TCP / IP information, etc. In some embodiments, a one-to-one correspondence is set between a collaborative control instance task and a slave robot arm. In this case, the collaborative control instance task corresponds to a unique second controller and a slave robot arm. In some embodiments, a one-to-one correspondence is set between a first communication instance task and a slave robot arm. In practical applications, when each second controller is configured with a master robot arm and a slave robot arm, the collaborative control instance task and the first communication instance task can also be set to correspond one-to-one with the second controller to achieve collaborative control of the slave robot arm.

[0076] Among them, such as Figure 6 The functions of each task shown are as follows: The instruction parsing task is mainly responsible for processing instructions sent from the host computer to the first controller. By setting the priority of the instruction parsing task to be lower than that of the collaborative control instance task, when some instructions from the host computer and the first controller involve blocking and waiting for return, even if there are operations that would cause the instruction parsing task to take too long and affect the motion control of the robot body, it will be temporarily preempted by higher priority real-time tasks and collaborative control instance tasks due to its lower priority, thus not affecting the motion control of the robot corresponding to each second controller.

[0077] The trajectory planning task is used for trajectory planning, and includes at least one of offline and online planning tasks. Offline planning tasks are primarily responsible for long-distance point-to-point trajectory planning, while online planning tasks are primarily responsible for tracking continuous trajectory point commands. In some embodiments, the offline planning task can plan an S-shaped velocity curve by inputting the robot's target pose, velocity parameters, acceleration parameters, and jerk parameters to obtain the motion trajectory from the current pose to the target pose, thus providing a convenient and quick trajectory planning method for simple paths. The online planning task can input pre-planned continuous short-distance trajectory points and trajectory point time interval information, and then smooth the data of several consecutive trajectory points, thus providing a trajectory planning method for complex paths and real-time trajectory tracking. Only one planning method can be used at a time. By prioritizing the trajectory planning task below the cooperative control task but above the command parsing task, it can still operate normally even when encountering complex situations that may require a large amount of computation, and in some cases, may fail to plan an effective trajectory.

[0078] Real-time motion control is the core task of robot motion control and the second highest priority task in the robot controller. It is primarily responsible for the motion control of the robot body corresponding to the various second controllers connected to the first controller. The input to real-time motion control comes from discrete trajectory points output by the planning task and joint information fed back from the EtherCAT communication task. When the input trajectory points come from the offline trajectory planning task, an adjustable-speed trajectory interpolation strategy can be used for trajectory interpolation; while when the input trajectory points come from the online trajectory planning task, a PD controller is used to implement trajectory tracking interpolation. This allows for different strategies to be used to interpolate the trajectory points output by the trajectory planning task based on different application scenarios. Taking Cartesian space trajectory interpolation and admittance control as an example, inverse kinematics can be used to obtain joint position commands, which are then stored in the data storage space. Afterwards, the real-time motion control task will enter a blocking state, waiting for a semaphore issued by the EtherCAT communication task. This semaphore is intended to ensure that the real-time motion control task does not modify the command content during EtherCAT communication transmission. Once the semaphore blocking is cleared by the EtherCAT communication task, the real-time motion control task receives feedback data (i.e., one of the control synchronization data) from the joint servo motors and force sensors. The force feedback data from the force sensors will be used for admittance control of the real-time motion control task, and the position data of the joint servo motors will be converted into Cartesian pose information through forward kinematics and used for admittance control and trajectory interpolation of the real-time motion control task.

[0079] The EtherCAT communication task is the most important and highest priority task in the robot controller. It is used to achieve hard real-time data exchange to meet the high-frequency (e.g., 1kHz) data interaction requirements of real-time motion control tasks and servo motor modules. It also needs to maintain distributed clock synchronization for multiple joint slave stations, ensuring that multiple joints sample and update instructions at the same microsecond level. The EtherCAT communication task reads data calculated by other tasks from the data storage space and stores the received data in the data storage space for subsequent use by other tasks. After completing a data exchange and time synchronization, the EtherCAT communication task enters a blocking state, waiting for a semaphore. This blocking is triggered by a preset interrupt (e.g., a 1kHz interrupt). The purpose is to strictly control the EtherCAT bus to communicate at the expected duration, while freeing up processor resources for other tasks to run. Because the EtherCAT communication task, real-time motion control task, and UDP communication task adopt a real-time streaming architecture, a semaphore is issued to release the real-time motion control task before the EtherCAT communication task completes data exchange and clock synchronization and enters the blocking state, and a semaphore is issued to release the UDP communication task's blocking state before the real-time motion control task enters the blocking state. At this time, EtherCAT communication tasks and other tasks will not be directly blocked on the same semaphore. When the preset interrupt releases the semaphore, each real-time task will be ready in sequence, solving the problem of priority inversion or execution order uncertainty.

[0080] The collaborative control instance tasks are key to achieving multi-robot collaborative motion. In some embodiments, when the first controller is the master controller, it will initiate a number of collaborative control instance tasks equal to the number of slave robotic arms. These tasks have the same priority as real-time motion control, and all collaborative control instance tasks have the same priority. The scheduling of each collaborative control instance task is controlled by a time-slice polling mechanism. Each collaborative control instance task is responsible for calculating and controlling the slave robot arm to follow the master robot arm in collaborative motion. When the collaborative control instance task receives trajectory point information generated by the trajectory interpolation module of the real-time motion control task, it uses it as the reference trajectory point of the master robot arm (i.e., the first end-effector pose information) and generates the reference trajectory point of the slave robot arm (i.e., the second end-effector pose information) based on the collaborative pose constraint information. Then, based on the offset generated by admittance control, the collaborative control instance task synchronously calculates the relative motion offset that the slave robot arm should generate, obtaining the pose information of the master and slave robots when the loose collaborative constraint conditions are met. At this point, the target Cartesian poses of the master and slave robots under tight and loose collaborative constraint conditions can be obtained respectively and stored in the robot instance of the data storage space, waiting for the UDP communication task to read the data and send it to the target slave controller. The collaborative control instance task is set as a non-real-time task because when the position command of the master robot arm has not changed, the position command of the slave robot arm generated by the collaborative control instance task will not change, and there is no need to set semaphore blocking, so it will not affect critical tasks such as EtherCAT communication task, real-time motion control, and UDP communication task. In this context, collaborative control instance tasks allow for longer execution cycles (e.g., 1-8ms) and, unlike real-time motion control tasks, do not obtain feedback information from the robotic arm in real-time via UDP communication. Therefore, collaborative control instance tasks can adopt an open-loop design. After the collaborative motion command is sent to the slave controller, the slave controller's real-time motion control task then performs more precise closed-loop control. For example, such as... Figure 6 As shown, there are m cooperative control instance tasks, namely cooperative motion control instance task 1 to cooperative motion control instance task m. Each cooperative control instance task determines whether to enter the tight cooperative process or the loose cooperative process based on the real-time motion control task. The tight cooperative process is the process that meets the tight cooperative constraints, and the loose cooperative process is the process that meets the loose cooperative constraints.

[0081] The UDP communication task is used to retrieve and send data calculated by the collaborative control instance task from the data storage space. Since multi-robot collaborative motion has high real-time requirements, the communication cycle between the master controller and the slave controller must be within 1-4ms, with jitter <100µs. If the UDP communication task's data transmission cycle is greater than 4ms, the slave robot arm will experience a significant start-up delay when following the master robot arm's movements. If the jitter exceeds 50% of the running cycle, the slave controller may not receive a new motion command after executing the previous one, causing the slave robot arm to stop. Simultaneously, to avoid the real-time motion control task being too time-consuming and affecting the real-time performance of UDP communication, the UDP communication task has a higher priority than the real-time control task but lower priority than the EtherCAT communication task. This application's embodiment, through a real-time streaming architecture design, enables the UDP communication task to run at a preset high frequency with a jitter of <100µs, meeting the hard real-time requirements for communication between the first and second controllers.

[0082] The TCP communication task is responsible for operations such as connecting, controlling, receiving, and disconnecting from the second controller to the first controller, and maintaining the TCP heartbeat between the master and slave controllers during coordinated motion. Since this task communicates with the second controller's TCP port via the TCP / IP protocol, it requires a relatively long blocking wait time, typically ranging from tens to thousands of milliseconds. Therefore, the TCP communication task from the first controller to other second controllers must be isolated from the more real-time motion control tasks, thus giving this task the lowest priority. The number of TCP communication tasks is equal to the number of robot instances belonging to the slave robotic arm in the data storage space. Multiple TCP communication tasks have equal priority, and a time-slice polling mechanism is used to coordinate the time each TCP communication task can occupy processor resources.

[0083] The anomaly detection task is used to collect error information (such as error codes and exception information) through the anomaly handling interface when the EtherCAT communication task and TCP communication task receive error messages from the first robot and the second controller, respectively. After capturing the error information, the anomaly handling task saves the robot's motion information from the previous cycle, including the Cartesian position, joint positions, and trajectory running time, and resets the anomaly information to the last saved result. If the TCP communication task returns an exception indicating TCP socket communication failure, it transitions the robot's state machine to a connected state. Finally, the anomaly handling task transmits the captured error codes and exception information to the host computer interface for display.

[0084] At this time, as Figure 6 As shown by the dashed arrows, taking the activation of trajectory tracking by the second controller as an example, the runtime sequence of each task in the current period t is as follows: In response to receiving a preset interrupt, the system determines that it has entered the current cycle t and sequentially starts the EtherCAT communication task, the UDP communication task, and the real-time motion control task. The EtherCAT communication task collects the real-time pose information of the main robotic arm of each second controller in the previous cycle t and stores it in the data storage space. Only then can the UDP communication task extract the second path planning result data of each second controller in the previous cycle t-1 from the data storage space and send it to each second controller at once. The real-time motion control task extracts the real-time pose data of the current cycle t from the data storage space and performs real-time trajectory planning processing to obtain the first planning result data and store it in the data storage space.

[0085] After the real-time motion control task is blocked, the non-real-time tasks are polled in a round-robin fashion according to their priority, as follows: 1) Traverse the robot state machine of each second controller, start and run the cooperative control instance task corresponding to the second controller whose robot state is running. After each running cooperative control instance task is completed, jump to step 2). Each activated cooperative control instance task executes the following steps during runtime: The cooperative pose constraint information is obtained from the data storage space. Based on the cooperative pose constraint information and the first planning result data updated in the data storage space in the current period t, the second end pose information is obtained. The second path planning result data determined by the first end pose information and the second end pose information is updated to the data storage space. 2) The trajectory planning task and the instruction parsing task are called sequentially; the trajectory planning task can obtain the discrete trajectory points for the next cycle, so that the real-time motion control task can perform trajectory point interpolation processing based on the discrete trajectory points in the next cycle. After the trajectory planning task and the instruction parsing task are completed, jump to step 3). 3) Traverse the robot state machine of each second controller, call the started first communication instance task, or for the unstarted first communication instance task, determine whether its corresponding robot state is connected. If it is connected, start and call the first communication instance task. After the first communication instance task has finished running, jump to step 4). Each called first communication instance task performs the following steps: When the second target controller is in the ready state, the second Cartesian pose data of the end of the slave arm corresponding to the second target controller and the first Cartesian pose data of the master arm are acquired. Based on the first Cartesian pose data and the second Cartesian position data, the cooperative pose constraint information is obtained and the robot state in the corresponding robot state machine is updated to the running state.

[0086] Reference Figure 7As shown in the embodiments of this application, a robot system is also provided, including: The first robot includes a first robot body and a first controller, and the first controller executes the robot system control method described above. Multiple second robots, each second robot including a second controller and a second robot body, the second controller being communicatively connected to the first controller; Both the first and second robot bodies include a master robotic arm and a slave robotic arm.

[0087] The embodiments of this application do not limit the number of second robots, such as Figure 7 As shown, there are 3 second robots and 1 first robot.

[0088] The first controller and the second controller can communicate via a switch.

[0089] In some embodiments, the robot system further includes a host computer, and the host computer and the robot are connected for communication.

[0090] A robot controller according to an embodiment of this application includes: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the robot system control method as described in any of the above embodiments.

[0091] For example, please refer to Figure 8 , Figure 8 The hardware structure of a robot controller according to another embodiment is illustrated. The robot controller includes: The processor 801 can be implemented using a general-purpose CPU (Central Processing Unit), microprocessor, application-specific integrated circuit (ASIC), or one or more integrated circuits, and is used to execute relevant programs to implement the technical solutions provided in the embodiments of this application. The memory 802 can be a NAND flash memory. The relevant program code is stored in the memory 802 and is called by the processor 801 to execute the robot system control method of the present application embodiment. The 803 input / output interface is used to implement information input and output. The communication interface 804 is used to enable communication and interaction between this device and other devices. Communication can be achieved through wired means (such as USB, Ethernet cable, etc.) or wireless means (such as mobile network, WIFI, Bluetooth, etc.). Bus 805 transmits information between various components of the device (e.g., processor 801, memory 802, input / output interface 803, and communication interface 804); The processor 801, memory 802, input / output interface 803, and communication interface 804 are connected to each other within the device via bus 805.

[0092] This application also provides a computer-readable storage medium that stores a computer program, which, when executed by a processor, implements the above-described robot system control method.

[0093] Memory, as a non-transitory computer-readable storage medium, can be used to store non-transitory software programs and non-transitory computer-executable programs. Furthermore, memory may include high-speed random access memory, and may also include non-transitory memory, such as at least one disk storage device, flash memory device, or other non-transitory solid-state storage device. In some embodiments, memory may optionally include memory remotely located relative to the processor, and these remote memories can be connected to the processor via a network. Examples of such networks include, but are not limited to, the Internet, intranets, local area networks, mobile communication networks, and combinations thereof.

[0094] The embodiments described in this application are for the purpose of more clearly illustrating the technical solutions of the embodiments of this application, and do not constitute a limitation on the technical solutions provided by the embodiments of this application. As those skilled in the art will know, with the evolution of technology and the emergence of new application scenarios, the technical solutions provided by the embodiments of this application are also applicable to similar technical problems.

[0095] Those skilled in the art will understand that the technical solutions shown in the figures do not constitute a limitation on the embodiments of this application, and may include more or fewer steps than shown, or combine certain steps, or different steps.

[0096] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs.

[0097] Those skilled in the art will understand that all or some of the steps in the methods disclosed above, as well as the functional modules / units in the systems and devices, can be implemented as software, firmware, hardware, or suitable combinations thereof.

[0098] The terms “first,” “second,” “third,” “fourth,” etc. (if present) in the specification and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of this application described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms “comprising” and “having,” and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0099] It should be understood that in this application, "at least one (item)" means one or more, and "more than" means two or more. "And / or" is used to describe the relationship between related objects, indicating that three relationships can exist. For example, "A and / or B" can represent three cases: only A exists, only B exists, and both A and B exist simultaneously, where A and B can be singular or plural. The character " / " generally indicates that the preceding and following related objects are in an "or" relationship. "At least one (item) of the following" or similar expressions refer to any combination of these items, including any combination of single or plural items. For example, at least one (item) of a, b, or c can represent: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", where a, b, and c can be single or multiple.

[0100] In the several embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of the units described above is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between apparatuses or units may be electrical, mechanical, or other forms.

[0101] The units described above as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0102] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0103] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes multiple instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this application. The aforementioned storage medium includes various media capable of storing programs, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0104] The preferred embodiments of the present application have been described above with reference to the accompanying drawings, but this does not limit the scope of the claims of the present application. Any modifications, equivalent substitutions, and improvements made by those skilled in the art without departing from the scope and substance of the embodiments of the present application shall be within the scope of the claims of the present application.

Claims

1. A robot system control method, characterized in that, The robot system control method is applied to a first controller, and the method includes: Multiple real-time tasks and multiple non-real-time tasks with different priorities are constructed. The multiple non-real-time tasks include multiple instance tasks. Each instance task is set to correspond to a second controller. The priority of the real-time tasks is higher than that of the non-real-time tasks. Each second controller is communicatively connected to the first controller. In response to receiving a preset interrupt, the real-time tasks and instance tasks are scheduled in a round-robin fashion according to the priorities of the real-time tasks and the non-real-time tasks. Each instance task is scheduled after the real-time task completes its corresponding business operation. Each real-time task is activated by acquiring its corresponding semaphore to complete its corresponding business operation. Each semaphore is released sequentially according to its corresponding priority.

2. The robot system control method according to claim 1, characterized in that, The multiple real-time tasks include EtherCAT communication tasks, User Datagram Protocol (UDP) communication tasks, and real-time motion control tasks; the scheduling of each real-time task includes the following steps: In response to receiving a preset interrupt, a first semaphore is released to the EtherCAT communication task to activate the EtherCAT communication task. Before the EtherCAT communication task completes the interactive processing of the control synchronization data for the current cycle and enters the blocking state, a second semaphore is released to the UDP communication task to activate the UDP communication task; the interactive processing of the control synchronization data includes updating the real-time pose data of the second controller to a preset data storage space. Before the UDP communication task sends the second path planning result data of the previous cycle to the corresponding second controller and enters the blocking state, a third semaphore is released to the real-time motion control task to activate the real-time motion control task. The real-time motion control task performs real-time trajectory planning processing on the real-time pose data stored in the data storage space in the current period, and stores the first planning processing result data into the data storage space. The path planning result data is determined based on the planning processing results.

3. The robot system control method according to claim 1, characterized in that, The plurality of real-time tasks include real-time motion control tasks and real-time communication tasks; the real-time communication tasks include at least one of EtherCAT communication tasks and UDP communication tasks; the plurality of instance tasks include collaborative control instance tasks, and each of the collaborative control instance tasks performs the following steps: The first end pose information of the first target controller corresponding to the collaborative control instance task is obtained from the first path planning result data stored in the preset data storage space. The path planning result data is obtained by the real-time motion control task performing real-time trajectory planning processing on the real-time pose data of the main robotic arm of each second controller in the current cycle. Obtain the cooperative pose constraint information of the first target controller in the current period; Based on the first end-effector pose information and the cooperative pose constraint information, second end-effector pose information corresponding one-to-one with the slave robot arm of the first target controller is obtained. The second path planning result data of the first target controller is obtained based on the first end pose information and the second end pose information of the same first target controller and updated to the data storage space, so as to notify the first target controller of the second path planning result data through the real-time communication task.

4. The robot system control method according to claim 3, characterized in that, The plurality of instance tasks also includes a first communication instance task, which has a lower priority than the collaborative control instance task; each of the first communication instance tasks performs the following steps: Acquire the first Cartesian pose data of the end of the main robotic arm of the second target controller corresponding to the first communication instance task, and acquire the second Cartesian pose data of the end of the robotic arm from the second target controller in real time; Based on the first Cartesian pose data and the second Cartesian pose data, the cooperative pose constraint information is obtained and updated to the data storage space; The first Cartesian pose data is used to indicate the pose information of the target object's centroid coordinate system in the end-effector coordinate system of the main robot arm during the current cycle, and the second Cartesian pose data is used to indicate the pose information of the end-effector free coordinate system of the robot arm in the centroid coordinate system of the target object during the current cycle. Alternatively, the first Cartesian pose data is used to indicate the pose information of the world coordinate system in the end-effector free coordinate system of the master robot within the current cycle, and the second Cartesian pose data represents the pose information of the end-effector free coordinate system of the slave robot in the world coordinate system.

5. The robot system control method according to claim 4, characterized in that, The first Cartesian pose data and the second Cartesian pose data of the end effector of the main robotic arm are acquired when the trajectory tracking enable state indicates that trajectory tracking is enabled and the trajectory tracking state is inactive. Each first communication instance task also performs the following steps: After obtaining the cooperative pose constraint information, the trajectory tracking state of the second target controller is activated so that each cooperative control instance task is invoked when the trajectory tracking state of the corresponding second target controller is activated.

6. The robot system control method according to claim 1, characterized in that, The first controller stores a robot state machine corresponding to each second controller. The step of polling and scheduling the real-time tasks and instance tasks according to their priorities includes: Based on the semaphore corresponding to each real-time task, each real-time task is scheduled according to its corresponding priority order. After each real-time task is scheduled, for each instance task, within the time slice divided according to the corresponding priority, the instance task is scheduled according to the robot state of the current period determined by the corresponding robot state machine.

7. The robot system control method according to any one of claims 2 to 4, characterized in that, The data storage space includes data instance storage spaces that correspond one-to-one with the second controller. Each data instance storage space is used to store instance data corresponding to the second controller. The instance data includes at least one of real-time pose data, first planning processing result data, second planning processing result data, and cooperative pose constraint information.

8. The robot system control method according to claim 4, characterized in that, The non-real-time task also includes an anomaly detection task, which has a lower priority than the first communication instance task. The anomaly detection task performs the following steps in the time slice allocated by the corresponding priority. Capture error controller exception information reported by the real-time task and / or instance task to obtain the capture result; If, based on the capture results, it is determined that there is at least one abnormal data from a third target controller, the motion parameters of the third target controller in the previous cycle are recorded as the last time in the normal state and saved in the preset data storage space. The abnormal data is interacted with external devices.

9. A robot controller, comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that the processor, when executing the computer program, implements the robot system control method as described in any one of claims 1 to 8.

10. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the robot system control method according to any one of claims 1 to 8.

Images