Dynamic Modelling and Motion Controller Design for Industrial Palletizing Robot and Collision Detection

The 4-DOF parallelogram industrial palletizing robot is taken as the object of study. This paper designs and develops the real-time industrial controller based on the IPC and high-way EtherCAT fieldbus. What’s more, the control algorithm and structure based on the dynamic feedforward contributes to the compensation of robot dynamics in theory. Experiments on the task switching and master delay show that this control system has a good real time response quality. Based on the kinematics analysis of robot parallelogram structure, this paper builds the robot dynamic model. To increase the computing speed and ensure the real-time performance, the model has been simplied and proved to be effective by simulation contrast verification.


Introduction
With the rapid development of science and automation technology, high-speed heavy-duty industrial robots have been more and more widely used in the industrial field, especially in the vehicle manufacturing and palletizing field. Accompanied with the leapfrog progress of robot technology, the safety issues caused by it have also raised more and more attention. At present, most researches focus on the safety of collaborative robots which directly interacts with the operators. In order to reduce and avoid the harm of accidental collisions to operators during interactive operations, many scholars have proposed different collision detection methods, including external sensor detection [1][2] , dynamic model detection [3][4][5] and so on. Compared with collaborative robots, palletizing robots are limited to work in fences when performing present tasks. However due to their fast speed and large load capacity, the palletizing robots usually have a wrist payload of more than 100kg and their peak linear velocity reaches 1m/s or more. Considering the unpredictability in the work, it will bring unpredictable damage to the surrounding environment and huge risks on the safety of operators when the accidents and collisions occur. Therefore, the research on the safety of palletizing robots also cannot be ignored. Now most of the palletizing robots commonly used in the industrial markets are based on the parallelogram structures, mainly including hybrid screw mechanism and double parallelogram structures [6] . The controllers are usually made up of the general-purpose processors and motion control cards where realize the HMI and motion control function respectively [7][8] . The drive controllers work in the position loop and send the position commands directly. At this time, the online calculation load of the motion controller is small and there is no high requirement of the control cycle and sampling frequency. However, due to the friction and inertia forces inside the robots' own joints, especially for high-speed heavy-load robots, the robot's inertia changes greatly and the nonlinear effects are significant, which makes that the traditional general-purpose processors combined with the motion control cards and the single linear controllers such as PID control law are difficult to guarantee the robot's motion performance and result in large position errors [9][10] . In order to improve the robot's motion performance, scholars have proposed a variety of complex control algorithms [11][12][13] . The implementation of most algorithms requires the control system to complete all calculations and send signals to the actuator in each sampling cycle. The similar approaches always bring huge computational loads and complex operations that make most of the methods are not practically applicable in industrial sites. In this paper, the 4-DOF parallelogram industrial palletizing robot is taken as the object of study. This paper designs and develops the real-time industrial controller based on the IPC and high-way EtherCAT fieldbus. Also a collision detection method based on moment residual threshold is presented and verified effectively by the simlution and practical experiments. The rest of this paper is organized as follows. In Section 2, the control system architecture based on the industrial computer and EtherCAT field bus is described. In Section 3, a brief kinematics analysis of parallelogram palletizing robot is presented and Section 4 contains the methods of dynamic modeling of palletizing robot. Section 5 details our experiments designed to verify the effectiveness of our model and finally we offer a brief summary in Section 6.

Robot Controller Architecture Based on Industrial Computer and EtherCAT Fieldbus
Industrial robot motion control system is divided into many different types mainly according to its control structure, motion command generation, task execution method, closed-loop feedback (position loop, current loop, etc) adjustment position and communication method. For manual teaching tasks, the control system does not require real-time hardware interaction. Most of them run in non-real-time operating systems with very friendly human-computer interaction interfaces. The servo closed-loop feedback of the motion control system includes the feedback adjustment in the position loop and the speed loop which needs online real-time calculation. For high-performance motion control systems, the sampling frequency of the position loop can reach 1kHz or higher while the sampling frequency of the speed loop is generally more than twice of the position loop. For high-speed and heavy-load industrial robots, the closed-loop calculation of servo feedback is more complicated and requires a higher sampling frequency. As to the controller architecture of the palletizing robot in this paper, we mainly consider the following two aspects,  control architecture  communication method If the servo driver of the robot works in the position loop, the online calculation load of the motion controller (motion control card or industrial control, etc.) is usually relatively small. At this time, the control system can ignore any dynamic coupling and nonlinear effects and each joint is in the decoupling control state. However, if the industrial robot works in a high-speed and heavy-load condition, the strong internal moment of inertia coupling between the joints and the impact of its dynamic effects always cannot be ignored. In order to achieve high-quality motion performance and simultaneously solve the problem of strong coupling between the joints, the application of dynamic feedforward compensation algorithm is indispensable and control decoupling between multiple joints can be achieved. At this time, each driving joint is considered as an independent control unit. In order to further improve the system's control performance, the controller (industrial computer or motion control card) usually completes the calculation of the complex control algorithm, while the joint servo driver only completes the feedback adjustment of the current loop. The higher sampling frequency of the driver's current loop contributes to better performance, but it will introduce a huge computational load and data interaction. The ordinary general-purpose analog signal processor obviously cannot meet the calculation demands, while the control card with a powerful digital signal processor is expensive. Then the hardware architecture of controller is considered and mainly composed of the industrial computer with a real-time system. Such architecture can not only achieve good real-time performance, but also meet the computing load requirements of most servo control algorithms. Compared with the traditional control architecture comprised of processors and motion control cards, the hardware architecture based on industrial computer not only simplifies the control board and A / D conversion module, but also makes the control structure more concise and improves the stability of the control system. The lack of speed information based on pulse signal position command interaction makes the speed command discontinuous and results in unstable system motion. The transmission of control commands CNAI2019 IOP Conf. Series: Materials Science and Engineering 790 (2020) 012157 IOP Publishing doi:10.1088/1757-899X/790/1/012157 3 based on analog signals is mostly affected by electromagnetic interference and other noise interference will be introduced into the system to reduce the control performance. High-speed fieldbus communication method not only solves the problem of interference during signal transmission, but also simplies the connection. Generally the minimum communication cycle reaches microsecond level and high-speed sampling frequency can be achieved.  Figure 1. Cortroller structure of industrial palletizing robot.
Based on the analysis of the above two aspects, we design a palletizing robot controller architecture based on industrial computer and EtherCAT bus. The architecture is shown in figure 1. We considering the robots working in the high-speed and heavy-load operating environment, a dynamic feedforward compensation algorithm framework is designed and shown in figure 2. All servo control commands (kinematic and dynamic model calculations) are generated by the industrial computer. Taking the advantage of the EtherCAT high-speed fieldbus, the torque command is directly sent to the servo drive and all feedback signals are fed back to the industrial computer in the same execution cycle. In order to compensate for the nonlinear effects caused by the dynamic coupling of the robot such as internal inertial forces, a dynamic feedforward control algorithm is introduced to realize theoretical compensation of the robot dynamic characteristics. At this time, the control object is no longer the joint position, but the torque and current controlling the drive components directly. As to communication methods, we take use of the EtherCAT high-speed bus, which combines the communication protocols of motion control buses (such as CANopen) with the bandwidth of industrial Ethernet, and maks its communication bandwidth as high as 100 Mbps [14][15] . In each working cycle, the data frames are transmitted on-the-fly in real time. Each slave station simultaneously completes the reading of data frames on the bus and the transmission of feedback data and achieve a nanosecond EtherCAT bus usually consists of two parts, the master station and the slave station. The master station runs on the real-time system or the core to ensure its response speed. This paper uses IntervalZero EtherCAT master based on RTX64 and Kingstar Motion. The KingstarMotion runs in the RTX64 real-time kernel. RTX64 implements the extension of the Windows kernel at the hardware abstraction layer through kernel extension technology. Then the Windows system and RTX64 kernel can run on the same industrial computer as shown in figure 1. The human-computer interaction interface runs in the Windows system, while the real-time control program runs, compiles, analyzes, and configures in the RTX64 kernel. The two parts exchange data through shared memory so that all data can be exchanged in real time on the Windows system.

Mechanism Introduction
The schematic diagram of the palletizing industrial robot mechanism is shown in figure 3(a), which consists of a set of main parallelogram ACDE and two sets of auxiliary parallelograms AKLE and EMNF. The main parallelogram ACLE determines the position of the end point F and the two sets of auxiliary parallelograms keep the end effector (point G) perpendicular to the horizontal plane. The motor that drives the main parallelogram is mounted on the turntable and controls the parallelogram position respectively by the synchronous pulley and the translation of linear screw which are marked as joint 2 and joint 3. The corresponding linear movement distances are u and v. The swivel is driven by a fixed motor and labeled as axis 1. Its corresponding joint angle is 1  . The first three joints consisting of axis 1 and joint 2 and 3 determine the position of end point F. A motor is added at the last joint (point G) and the last joint rotates the end effector and operating object and is labeled as axis 4. Its corresponding joint angle is 2  . The robot coordinate system is shown in figure 3. {OXYZ} is the world coordinate system and{oxyz}is a

Kinematic Analysis
Let the coordinates of the robot end point F is ( , , ) According to the plane geometric relationship of the mechanism (similar triangles), Simultaneous equation (1) and (2) can be obtained cos cos Adding the squares of the two formulas in equation (3), we get The angle  and  can be obtained based on equations (4) and (3)

Dynamic Model
As can be seen from figure 3(a), the robot contains 13 parts in addition to the fixed base: 1 rotating base, 2 sliders A and B, 4 main links DB, DF, AC and AE, 4 The auxiliary links AK, KL, LME and MN, as well as the end platform FN and the end grip G. are the gravity centers of the four main links. Due to the introduction of the secondary linkage, the subsequent dynamic model calculation is more complicated. The dynamic model needs to be simplified. The subsequent sections verify that the simplification calculation has little effect on the results by the contrastive verification with Adams simulation and theoretical calculation. The simplification of the dynamic model mainly starts from the simplification of auxiliary parallelogram and end joint.

 simplification of auxiliary parallelogram
Because several auxiliary links have small masses relative to the main links and the relative speed to the main links is low, this part can be simplified. That is, the mass and inertia of the auxiliary links KA, KL are converted to the main link AE and the mass and inertia of the auxiliary links LME, MN are converted to the main link DF.  simplification of end joints As to the palletizing robot, the end gripper is usually grasping the position on the item that is near the center of gravity. Otherwise, it cannot be grasped steadily. The rotation of last joint does not change the position of the gravity center. In Lagrange's equation, the gravity potential energy needs to be derived from each generalized coordinate and its rate of change (velocity). Meanwhile the end joint NFG keeps horizontal in the dynamic coordinate system. Therefore,  6 we set the position of gravity center of end joint NF and the item to coincide. Such simplification has no effect on the results of the dynamics solution. The simplified schematic diagram of robot mechanism is shown in figure 3(b), where 1 2 3 4 , , , C C C C     is the overall gravity center of each link after conversion.

Computational Analysis of Mechanism Dynamics Model
Modeling the robot according to the Lagrangian method, the basic equation is as follows Let L be the Lagrangian constant, that is, the difference between the system kinetic energy T and potential energy P. i q and i  are generalized coordinate and moment of the ith joint, respectively. The overall kinetic energy (potential energy) of system is the sum of the kinetic energy (potential energy) of each joint (link, slider, end load, etc.). For any link, the kinetic and potential energy expressions are given by where, subscript c represents the gravity center of each link. The rotational kinetic energy can be calculated by the sum of rotational kinetic energy in the direction of three inertia principal axes, that is given by The inertia tensor of each link should be determined by the gravity center. The inertia tensor I and angular velocity should be described in the same coordinate system, that is given by For the sliders of joints 2 and 3 and taking slider A as an example, the calculation expressions of kinetic energy and potential energy are given by   For the end joint NF and load G, it is assumed that the end mass is e m , the load mass is M, the rotational inertia of rotation axis is G I , and the mass center is on the fourth joint rotation axis and not eccentric.
Then its kinetic and potential energy calculation expressions are given by Using the same method, the kinetic and potential energy expressions of all links, sliders and other components are obtained. Substituting them into Lagrange's equation (6), the four generalized driving forces and torques can be obtained as follows, , where,  is the generalized driving force or torque of system, ( ) M q is inertia matrix, ( , ) V q q  is centrifugal and coriolis force matrices, and is gravity matrix.

Robot Collision Detection Based on Dynamics Model
In order to realize that the palletizing robot can stop working immediately when an accidental collision occurs to ensure the safety of operators and surrounding object, this paper proposes a robot collision detection method based on the torque residual threshold of dynamic model. By using the sensors inside the palletizing robot without the external sensor measurement, this method not only directly reduces the detection cost, but also reduces the complexity of the whole system and makes it stable. When the robot collides with the other objects, a sudden change in torque occurs compared to the normal movement. Therefore, given the robot motion function and substituting the robot kinematic and dynamic parameters, the driving torque of each joint based on the dynamic model can be calculated according to equation (12). When the residuals e  of output torque real  and set  of the actual robot in continuous motion exceed a given threshold e , we believe that the robot has an accidental collision at this time, and immediately stop working. The dynamic parameters of equation (12) are obtained through the CAD model. Due to the inaccuracy of internal model parameters and external friction interference, there are errors between the driving force and torque calculated based on the dynamic analytical model and the output force and torque of actual robot. Therefore, the dynamic model of equation (12) needs to be further identified and optimized.
When considering the internal and external interference in the actual motion, equation (12) is rewritten as follows, where, f  is the interference to the system due to friction and other factors. The friction model has been studied by many scholars. However, some states such as commutation still cannot be accurately described. Generally, most friction characteristics can be characterized by the classic Coulomb-viscous friction model.  (15) is solved by the least square method.  is regarded as the calculated estimated torque set  in a given motion state after the model identification and optimization. It is compared with the actual output torque real  to obtain the torque residual and then the torque residual is compared with threshold to estimate whether a collision occurs.

Motion Controller Real-time Test
To verify the real-time performance of the designed motion controller based on industrial computer and EtherCAT field bus, we tested task switching delay of the controller real-time system and EtherCAT master operation and network delay respectively. Task switching delay refers to the time difference between switching to the next task after the previous task process ends when multiple tasks run on the same processor. The EtherCAT master operation delay is the time difference between the master station sending a command and the frame actually sending it. The EtherCAT master network delay is the time interval between the time that is sent from the master and the time when data frame traverses all the slaves and returns to the master again. The EtherCAT master operation and network delay are obtained from the data frame timestamps which are obtained by the Wiresharp software at various moments and calculated by the difference. The IPC configuration consists of Intel Celeron J1900, 4GB of memory where Windows Embedded Standard 7 (SP1, 64-bit version) is installed and the RTX64 real-time kernel of IntervalZero is extended. The EtherCAT master KingstarMotion run in the IPC. The master cycle is set to 1ms. The IPC and 4 slave drives are connected in series as shown in Fig. 1. The experiment was performed twice. Each time 1000 groups of data were tested and their maximum, minimum and average values were calculated for comparison. The test results are shown in Table 1. It can be seen from the results that both of the task switching delay and operation delay of the master station are in the order of microseconds. As to the 1ms frame cycle set in the experiment, there is a certain amount of jitter in the EtherCAT communication network. But the amplitude of the jitter is small which has basically no effect on the communication cycle. The EtherCAT communication method in this experiment is in a serial logical ring communication. The message is sent from master station and finally returns to the master. There is a certain network delay during this period, but its value is very small at the sub-microsecond level. Considering that the control cycle of industrial robots is usually around 1ms, the delays can fully meet the real-time control requirements of industrial robots and collision detection.

Kinematic Model Validation
Contrast the kinematics model built with the Solidworks 3D model and Matlab, we give the fixed joint motion distances u and v respectively and observe the changes in  and  angles when another input quantity changes. It can be known from equation (5)

Conclusion
This paper extends the safety of lightweight collaborative robots to the field of high-speed heavy-duty industrial robots. For industrial palletizing robot with hybrid screw mechanism, we design and develop its motion controller based on industrial computer and real-time kernel and high-speed EtherCAT We not only simplify the control system architecture, but also improve the real-time response performance and stability of the controller. The following experiments verify that the controller fully meets the real-time requirements of motion control for collision detection applications. Taking advantage of the characteristics of the parallelogram mechanism, we derive a linear analytical form of the robot dynamics model and the model is further optimized and identified. What's more, we propose the collision detection method based on the dynamic model and torque residual threshold for the highspeed heavy-duty industrial palletizing robot. Combined with the practical application of palletizing robots, this paper eliminates the impact of sudden torque changes on the collision detection when the speed commutation occurs. The collision detection method is especially applicable to the scenarios where the robot repeatedly handles heavy-duty materials.