An open multi-execution end control system and method based on TwinCAT software

Abstract

The present invention discloses an open multi-execution end control system and method based on TwinCAT software, including a human-machine interface system and a control system developed based on TwinCAT software, the system including an execution end controller, a trajectory planning core, a receiving buffer, a planning buffer, a trajectory interpolation core, and a servo driver; the human-machine interface system performs parallel operation of multiple execution ends through QT multi-threaded operation and NC master control rules; the trajectory planning module and the trajectory interpolation module of each execution end are distributed in different cores; the execution end realizes first-in-first-out storage through the switching of multiple buffers, and avoids writing the read lock of the multi-core system on the shared variables by staggered access to different buffers, and realizes online trajectory planning and interpolation by running the trajectory planning core and the trajectory interpolation core at the same time. The present invention can effectively realize the simultaneous online trajectory planning and interpolation control of multiple execution ends, and expand the application scenarios and research of multi-execution end devices.

Description

Technical Field

[0001] The present invention belongs to the technical field of collaborative control of multiple execution terminals, and specifically relates to an open multi-execution terminal control system and method based on TwinCAT software. Background Art

[0002] When modern aircraft are integrated and assembled, the front, middle and rear sections of the fuselage need to be docked, adjusted and assembled, which involves the complex movements of multiple supporting actuators. By driving multiple supporting actuators to move at the same time, the stress deformation of components caused by the movement of a single actuator can be reduced, and the time consumption caused by the sequential execution of multiple actuators can be reduced. In addition, there are more and more collaborative working modes of multiple actuators, such as the increasing number of multiple robots in factories that work together to complete a task. However, commercial multi-actuator control systems are closed and cannot be secondary developed to meet specific needs. Therefore, how to develop an open control technology that controls multiple actuators to perform complex movements is a problem to be solved.

[0003] Existing multi-execution end control technologies, such as the patent with application number 202310699731.9, which proposes a control method for a multi-robot collaborative cabin intelligent drilling and riveting system, focus more on the multi-robot work at the task level, and cannot realize the continuous trajectory simultaneous movement of multiple robots, and does not involve the logical implementation of the underlying control system of multiple robots; in terms of single-execution end control systems, the patent with application number 202010483577.8 proposes a control system for serial-parallel robots and a robot control system design, but this method cannot realize online continuous trajectory interpolation, and the trajectory information needs to be interpolated and inverted in advance, and the angle information needs to be sent to the servo drive. This method increases the amount of code in the host computer system. Moreover, it is more complicated for TwinCAT to implement the "first-in, first-out" queue mode using ST language. On the whole, there is an urgent need for an open multi-execution end online control system. Summary of the invention

[0004] The technical problem to be solved by the present invention is to address the deficiencies of the above-mentioned prior art and to provide an open multi-execution end control system and method based on TwinCAT software. The open structure of a human-machine interface plus a control system is adopted to overcome the problem that traditional open control systems cannot be planned online, and parallel control of multiple execution ends is realized. It can effectively realize simultaneous online trajectory planning and interpolation control of multiple execution ends, and expand the application scenarios and research of multi-execution end devices.

[0005] In order to achieve the above technical objectives, the technical solution adopted by the present invention is:

[0006] An open multi-execution end control system based on TwinCAT software, characterized in that it includes a human-machine interface system and a control system developed based on TwinCAT software, wherein the control system developed based on TwinCAT software includes an execution end controller, a trajectory planning core, a receiving buffer, a planning buffer, a trajectory interpolation core, and a servo driver;

[0007] The human-machine interface system is used to perform NC program loading, decoding, manual operation and program startup, and to perform parallel operation of multiple execution terminals through QT multi-threaded operation and NC master control rules;

[0008] The execution end controller realizes first-in first-out storage by switching multiple buffers, and staggers access to different buffers;

[0009] The corresponding receiving buffer of the trajectory planning core of the execution end controller communicates with the human-machine interface system to receive data sent by the human-machine interface system;

[0010] The trajectory planning core is used to read data from the receiving buffer to perform trajectory planning and store the data in the planning buffer;

[0011] The trajectory interpolation core is used to read the planned data for interpolation, and send the inverse solved data to the corresponding servo driver for movement.

[0012] To optimize the above technical solutions, the specific measures taken also include:

[0013] The above-mentioned human-machine interface system is developed based on QT software, and the corresponding receiving buffer of the trajectory planning core of the execution end controller transmits data through ADS.

[0014] When the NC program in the above-mentioned human-machine interface system is started, whether the NC program of each execution end is executed is determined by the overall NC scheduling program;

[0015] When the NC program of the corresponding execution end is to be executed, the human-machine interface system activates a thread from the thread pool to execute the NC program of the corresponding execution end, and when not in use, the corresponding thread is recycled to the thread pool, and the number of threads running at the same time is less than or equal to N+1, where N is the number of execution ends controlled;

[0016] The human-machine interface system marks each NC code after it is executed by the execution end and a signal is fed back.

[0017] The trajectory planning core and trajectory interpolation core corresponding to each of the above-mentioned execution end controllers are distributed in different cores, and the priority of the trajectory interpolation core is higher than that of the trajectory planning core, and multiple cores run independently at the same time; the trajectory planning core and the trajectory interpolation core use the global variable area for data transmission; the receiving buffer of the trajectory planning core receives the processing data decoded by the human-machine interface system.

[0018] The receiving buffers corresponding to each of the above-mentioned execution end controllers are two fixed-length receiving buffers 1 and 2. When receiving buffer 1 is full, receiving buffer 2 will start receiving data for storage; when the data in receiving buffer 1 is taken away, the data in receiving buffer 2 can be taken away;

[0019] When all the data in receive buffer 1 or receive buffer 2 is taken away, a signal is sent to the human-machine interface system to refill the data;

[0020] Each time the controller is restarted, each execution end controller first fills up its corresponding receiving buffer 1 and then starts trajectory planning.

[0021] When the above trajectory planning core reads data from the receiving buffer for trajectory planning, it performs the following data judgment:

[0022] If three adjacent data points are collinear in the same direction, the middle point data is removed and the straight line data is merged until data that satisfies the first planning is obtained; if three adjacent data points are collinear in a reversing manner, the last data of the three adjacent data points is put into the next planning data set, and the data volume of the current planning is supplemented;

[0023] The trajectory planning includes the construction of a smooth corner curve, the shortest time global speed continuous planning, interpolation cycle rounding, and posture synchronization; the planned data is sent to the planning buffer of the corresponding execution end controller to wait for the interpolation of the trajectory interpolation core.

[0024] Each of the above-mentioned execution-end controllers corresponds to two fixed-length planning buffers 1 and planning buffer 2. Each execution-end controller must first take data from planning buffer 1, and only after all the data in planning buffer 1 are taken out can it take data from planning buffer 2; when the data in planning buffer 1 is taken out, the mark bit is sent to the trajectory planning core, waiting for the planning data to be filled again; when each execution-end controller re-plans, it is necessary to wait for planning buffer 1 to be filled before reading data for interpolation operations to avoid data reading conflicts.

[0025] The trajectory interpolation core reads the planned data from the planning buffer for interpolation. The data taken away include the start and end point positions and speeds of the current segment, the time of each speed curve, and additional variables. The position of each interpolation cycle is calculated according to the speed planning curve and corner smooth curve model corresponding to the trajectory planning core, and the calculated position is sent to the inverse solver of the corresponding execution end controller for inverse solution. Then the data is sent to the servo drive through the EtherCAT bus for movement, thereby realizing the movement of the execution end.

[0026] The trajectory planning core continuously scans whether there is a completely empty planning buffer while performing interpolation, thereby achieving simultaneous operation of planning and interpolation.

[0027] After the execution of the above-mentioned execution end controller task is completed, all planning variables are set, including:

[0028] After the execution end controller completes the motion task at the current stage, all trajectory planning and trajectory interpolation as well as the variables and data in the planning buffer that communicate with the human-machine interface system are set, waiting for the next execution end task to be executed.

[0029] An open multi-execution end control method based on TwinCAT software, comprising:

[0030] Step 1: The human-machine interface system simultaneously imports the running programs of multiple execution terminals and starts running them;

[0031] Step 2: The corresponding receiving buffer of the trajectory planning core of the execution end controller communicates with the human-machine interface system to receive data sent by the human-machine interface system;

[0032] Step 3: The trajectory planning core reads data from the receiving buffer to perform trajectory planning and stores it in the planning buffer;

[0033] Step 4: The trajectory interpolation core reads the planned data for interpolation and sends the inverse solved data to the corresponding servo drive for motion;

[0034] Step 5: After the task of the current execution end is completed, all variables are set and wait for the next movement.

[0035] The present invention has the following beneficial effects:

[0036] 1. The human-machine interface system of the present invention realizes the distribution of multiple NC processing texts through the multi-threaded operation of QT software and the NC master control text rules, can realize the NC parallel motion control of multiple execution ends, can realize the processing of any line segment trajectory, and will not increase the amount of NC code of the human-machine interface.

[0037] 2. The present invention provides an implementation strategy for the underlying control logic of the execution end, effectively realizing the parallel operation of multiple execution ends, and realizing online trajectory planning and interpolation control of each execution end by independently running the trajectory planning core and the trajectory interpolation core at the same time. The trajectory planning module and the trajectory interpolation module of each execution end are distributed in different cores of the TwinCAT software, and the two exchange data through the global variable area. The difficulty of using the ST language to implement the "first in, first out" register is avoided through the switching strategy between multiple buffers, and the conflict of the read-write lock of the shared variable by the multi-core program is avoided by staggered access to different buffers, thereby eliminating the operation of the read-write lock and realizing online continuous trajectory planning and control.

[0038] 3. The control system of the present invention is open, and can easily implement functions such as error compensation module and external IO module addition, and can effectively expand system functions. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] FIG1 is a schematic diagram of an open multi-execution terminal control system based on TwinCAT software according to the present invention. DETAILED DESCRIPTION

[0040] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0041] Although the steps in the present invention are arranged with numbers, they are not used to limit the order of the steps. Unless the order of the steps is clearly stated or the execution of a certain step requires other steps as a basis, the relative order of the steps can be adjusted. It is understood that the term "and / or" used in this article involves and covers any and all possible combinations of one or more of the associated listed items.

[0042] like Figure 1 As shown, the present invention provides an open multi-execution end control system based on TwinCAT software, including a human-machine interface system and a controller system developed based on TwinCAT software, wherein the control system developed based on TwinCAT software includes an execution end controller, a trajectory planning core, a receiving buffer, a planning buffer, a trajectory interpolation core, and a servo driver;

[0043] The human-machine interface system is used to perform NC program loading, decoding, manual operation and program startup, and to perform parallel operation of multiple execution terminals through QT multi-threaded operation and NC master control rules;

[0044] The execution end controller realizes first-in first-out storage by switching multiple buffers, and staggers access to different buffers;

[0045] The corresponding receiving buffer of the trajectory planning core of the execution end controller communicates with the human-machine interface system to receive data sent by the human-machine interface system;

[0046] The trajectory planning core is used to read data from the receiving buffer to perform trajectory planning and store the data in the planning buffer;

[0047] The trajectory interpolation core is used to read the planned data for interpolation, and send the inverse solved data to the corresponding servo driver for movement.

[0048] By running the trajectory planning core and the trajectory interpolation core simultaneously, online trajectory planning and interpolation can be achieved.

[0049] Based on the above open multi-executor control system, an open multi-executor control method based on TwinCAT software is proposed, which specifically includes the following steps:

[0050] Step 1: Import the running programs of multiple execution terminals into the human-machine interface system at the same time and start running;

[0051] Step 2: In the TwinCAT control system, the corresponding receiving buffer of the trajectory planning core of the execution end controller receives the data sent by the human-machine interface system;

[0052] Step 3: The trajectory planning core reads data from the receiving buffer for planning and stores it in the planning buffer;

[0053] Step 4: The trajectory interpolation core reads the planned data for interpolation and sends the inverse solved data to the corresponding servo drive for motion;

[0054] Step 5: After the task of the current execution end is completed, all variables are set and wait for the next movement.

[0055] In the embodiment, the human-machine interface system of step one is developed based on QT software, and is mainly used to perform non-real-time tasks, such as NC program loading, decoding, manual operation, program startup, etc., and data is transmitted between it and the execution end controller through ADS.

[0056] When the NC program is started, whether the NC program of each execution end is executed is determined by the overall NC scheduler;

[0057] When the NC program of the corresponding execution end is to be executed, the human-machine interface system will enable a thread from the thread pool to execute the NC program of the corresponding execution end. When not in use, the corresponding thread will be recycled to the thread pool. The number of threads running at the same time is less than or equal to N+1, where N is the number of execution ends controlled.

[0058] After each NC code is executed by the execution end, a signal will be fed back to the human-machine interface system to mark it green.

[0059] In the embodiment, the execution end controller of step 2 is developed based on TwinCAT software, and multiple independent cores are separated in TwinCAT software, and multiple cores can run independently at the same time. The trajectory planning core and trajectory interpolation core of each execution end are distributed in different cores, and the priority of the trajectory interpolation core is higher than that of the trajectory planning core, and multiple cores run independently at the same time.

[0060] The trajectory planning core and the trajectory interpolation core use the global variable area for data transmission.

[0061] The receiving buffer of the trajectory planning core receives the processing data after being decoded by the host computer.

[0062] The receiving buffer of each execution end is two fixed-length arrays. When receiving buffer 1 is full, receiving buffer 2 will start receiving data for storage; when all the data in receiving buffer 1 is taken away, the data in receiving buffer 2 can be taken away. When all the data in a receiving buffer is taken away, a signal is sent to the host computer to refill the data.

[0063] And each time it is restarted, each execution end must first fill up the receiving buffer 1 before starting trajectory planning.

[0064] In the embodiment, when the trajectory planning core of step 3 reads data from the receiving buffer for trajectory planning, data judgment is required. (in are three adjacent points), then the middle point needs to be removed Merge the straight line data until the data that satisfies the primary planning is obtained; if three points are collinear, , then the next point Put it into the next planning data set and make up the data volume of the current plan.

[0065] The content of trajectory planning includes the construction of smooth corner curves, the shortest time global speed continuous planning, interpolation cycle rounding, posture synchronization, etc. The planned data needs to be sent to the planning buffer of the corresponding execution end to wait for the interpolator's interpolation.

[0066] Each execution end has two fixed-length planning buffers, and each execution end must first fetch data from planning buffer 1. After all the data in planning buffer 1 is fetched, data from planning buffer 2 can be fetched. When all the data in planning buffer 1 is fetched, the marker bit will be sent to the trajectory planning core, waiting for the planning data to be filled again.

[0067] In addition, when each execution end re-plans, it is necessary to wait until the planning buffer 1 is fully filled before reading data for interpolation, so as to avoid data reading conflicts.

[0068] In the embodiment, the trajectory interpolation core of step 4 reads the planned data from the planning buffer for interpolation. The data taken includes the start and end point positions and speeds of the current segment, the time of each speed curve, additional variables, etc. The position of each interpolation cycle is calculated according to the speed planning curve and corner smooth curve model corresponding to the trajectory planning core. The interpolated position is sent to the inverse solver of the corresponding execution end for inverse solution, and then the data is sent to the servo drive through the EtherCAT bus for movement, thereby realizing the movement of the execution end. While interpolating, the trajectory planning core keeps scanning whether there is a completely empty planning buffer, thereby realizing the simultaneous operation of planning and interpolation.

[0069] In the embodiment, after the current execution end task of step 5 is executed, all planning variables are set, which means that after the execution end of the current stage completes the movement, all variables for trajectory planning and trajectory interpolation and communication with the host computer need to be set, especially the data in the buffer, waiting for the next execution end task to be executed. This avoids the situation where the next execution end trajectory is abnormal due to data residue.

[0070] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0071] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. An open multi-execution end control system based on TwinCAT software, characterized in that: It includes a human-machine interface system and a control system developed based on TwinCAT software, wherein the control system developed based on TwinCAT software includes an execution end controller, a trajectory planning core, a receiving buffer, a planning buffer, a trajectory interpolation core, and a servo drive; The human-machine interface system is used to perform NC program loading, decoding, manual operation and program startup, and to perform parallel operation of multiple execution terminals through QT multi-threaded operation and NC master control rules; The execution end controller realizes first-in first-out storage by switching multiple buffers, and staggers access to different buffers; The corresponding receiving buffer of the trajectory planning core of the execution end controller communicates with the human-machine interface system to receive data sent by the human-machine interface system; The trajectory planning core is used to read data from the receiving buffer to perform trajectory planning and store the data in the planning buffer; The trajectory interpolation core is used to read the planned data for interpolation and send the inverse solved data to the corresponding servo driver for movement; The receiving buffer corresponding to each execution end controller has a fixed-length receiving buffer 1 and receiving buffer 2. When receiving buffer 1 is full, receiving buffer 2 will start receiving data for storage; After all the data in receive buffer 1 have been taken away, the data in receive buffer 2 can be taken away; When all the data in receive buffer 1 or receive buffer 2 is taken away, a signal is sent to the human-machine interface system to refill the data; Each time it is restarted, each execution end controller first fills up its corresponding receiving buffer 1 and then starts trajectory planning; When the trajectory planning core reads data from the receiving buffer for trajectory planning, the following data judgment is performed: If three adjacent data points are collinear in the same direction, the middle point data is removed and the straight line data is merged until data that satisfies the first planning is obtained; if three adjacent data points are collinear in a reversing manner, the last data of the three adjacent data points is put into the next planning data set, and the data volume of the current planning is supplemented; The trajectory planning includes the construction of a smooth corner curve, the shortest time global speed continuous planning, interpolation cycle rounding, and posture synchronization; the planned data is sent to the planning buffer of the corresponding execution end controller to wait for the interpolation of the trajectory interpolation core; Each execution end controller corresponds to a fixed-length planning buffer 1 and a planning buffer 2. Each execution end controller must first fetch data from planning buffer 1, and only after all the data in planning buffer 1 is fetched can it fetch data from planning buffer 2. When the data in planning buffer 1 is completely taken, the marker bit is sent to the trajectory planning core, waiting for the planning data to be filled again; When each execution end controller re-plans, it needs to wait until the planning buffer 1 is fully filled before reading data for interpolation, so as to avoid data reading conflicts.

2. The open multi-actuator control system based on TwinCAT software according to claim 1, characterized in that: The human-machine interface system is developed based on QT software, and transmits data with the corresponding receiving buffer of the trajectory planning core of the execution end controller through ADS.

3. The open multi-actuator control system based on TwinCAT software according to claim 1, characterized in that: When the NC program in the human-machine interface system is started, whether the NC program of each execution end is executed is determined by the overall NC scheduling program; When the NC program of the corresponding execution end is to be executed, the human-machine interface system activates a thread from the thread pool to execute the NC program of the corresponding execution end, and when not in use, the corresponding thread is recycled to the thread pool, and the number of threads running at the same time is less than or equal to N+1, where N is the number of execution ends controlled; The human-machine interface system marks each NC code after it is executed by the execution end and a signal is fed back.

4. The open multi-actuator control system based on TwinCAT software according to claim 1, characterized in that: The trajectory planning core and trajectory interpolation core corresponding to each execution end controller are distributed in different cores, and the two exchange data through the global variable area, and the priority of the trajectory interpolation core is higher than that of the trajectory planning core, and multiple cores run independently at the same time; the trajectory planning core and the trajectory interpolation core use the global variable area for data transmission; the receiving buffer of the trajectory planning core receives the processing data decoded by the human-machine interface system.

5. The open multi-actuator control system based on TwinCAT software according to claim 1, characterized in that: The trajectory interpolation core reads the planned data from the planning buffer for interpolation. The data taken away include the start and end point positions and speeds of the current segment, the time of each speed curve, and additional variables. The position of each interpolation cycle is calculated according to the speed planning curve and corner smooth curve model corresponding to the trajectory planning core, and the calculated position is sent to the inverse solver of the corresponding execution end controller for inverse solution. Then, the data is sent to the servo drive through the EtherCAT bus for movement, thereby realizing the movement of the execution end. The trajectory planning core continuously scans whether there is a completely empty planning buffer while performing interpolation, thereby achieving simultaneous operation of planning and interpolation.

6. The open multi-actuator control system based on TwinCAT software according to claim 1, characterized in that: After the execution of the task of the execution end controller is completed, all planning variables are set, including: After the execution end controller completes the motion task at the current stage, all trajectory planning and trajectory interpolation as well as the variables and data in the planning buffer that communicate with the human-machine interface system are set, waiting for the next execution end task to be executed.

7. An open multi-executor control method of an open multi-executor control system based on TwinCAT software according to any one of claims 1 to 6, characterized in that: include: Step 1: The human-machine interface system simultaneously imports the running programs of multiple execution terminals and starts running them; Step 2: The corresponding receiving buffer of the trajectory planning core of the execution end controller communicates with the human-machine interface system to receive data sent by the human-machine interface system; Step 3: The trajectory planning core reads data from the receiving buffer to perform trajectory planning and stores it in the planning buffer; Step 4: The trajectory interpolation core reads the planned data for interpolation and sends the inverse solved data to the corresponding servo drive for motion; Step 5: After the task of the current execution end is completed, all variables are set and wait for the next movement.

Images