Design and Implementation of Admittance Control for a Dual-Arm Robot under Space Limitation

Aimed at the situation lack of suitable industrial robots with speed requirement and space limitation, a novel simple structured and high speed dual-arm robot is designed. The robot control system has been achieved by using high speed controller, real-time bus EtherCAT and integrating the sensor system via Ethernet interface. Kinematic and dynamic analysis are the basis of its kinematic control and trajectory planning. This paper presents a force-free control method for direct teaching of the robot and adopts a Cartesian admittance control algorithm to realize human-machine interaction. The admittance control is conducted by utilizing six-dimensional force/torque sensor fixed to the end-effector of manipulator. To evaluate the performance of the proposed controller and control algorithm, a point-to-point teaching task is conducted.


Introduction
Along with the development of industrial automation, industrial robots have wider applications in automatic field. As the space limitation of production environment and the diversity of tasks, the standard industrial robots with fixed dimension are not appropriate for some production conditions [1]. In the course of the production of small size ceramic tiles, there is a narrow space for industrial robot to grab and stack ceramic tiles. To satisfy the speed requirement and space limitation, a novel robot with two simple structured arms is designed to grab and stack small ceramic tiles.
In the process of traditional teaching, the robot can be taught and play back to track the work path by trained operators. Due to different production conditions and workspace restrictions of robot, traditional programming method for robot teaching is unable to fully satisfy the demands of the production process. Therefore, various teaching-playback methods have been proposed, and direct teaching is one of them [2]. There are several studies [3][4][5] proposed methods that change the impedance characteristics according to the estimated environmental stiffness to minimise the tracking error. Khansari-Zadeh and Khatib [6] presented a novel timeinvariant controller called unified motion and variable impedance control for robot demonstrations. Their method can create a safe and compliant robot controller. Impedance and admittance control algorithms have been studied for human-robot cooperation task [7][8][9][10]. Dimeas et al. [10] proposed a method of virtual Cartesian constraints to prevent the operator from guiding the manipulator to low-performance configurations. In addition, integrating manipulator performance constraints into Cartesian admittance control, which is easy to apply on the robot with a force/torque sensor attached to its end-effector.
This paper presents a force-free control method for robot teaching in a constraint condition. Besides, Cartesian admittance control algorithm is utilized to realize force-free control and expected force tracking. The applied force is sensed by the force/torque sensor fixed to the end-effector of manipulator, and the robot responds with an end-effector Cartesian velocity. In this way, an operator can move the end-effector of manipulator to a desired position or path under the workspace restriction. An experimental evaluation for newly designed dual-arm robot is conducted by using Cartesian admittance control. The effectiveness of the proposed control system is evidenced by numerical simulation and experimental results.

Mechanical design
The production process of small size ceramic tiles involves sorting, arranging, molding, stacking and packaging. Ceramic tiles are sintered in a kiln and fall into a certain mold after sorting. Before packaging, ceramic tiles need to be grabbed from the mold and then carried from molding line to packaging line. At present, most of those tasks need to be done manually. The existing molding lines transport the ceramic tiles mold through a cyclic transmission which molding ceramic tiles at the top of the molding line and grabbing from the bottom [11]. The production line of ceramic tiles is need to realize the task of grabbing and stacking 30 layers ceramic tiles per minute. To address the problems of grabbing and stacking ceramic tiles quickly in limited space, a simple structured dual-arm robot is designed [12]. Fig. 1 shows the three-dimensional model of the proposed robot has two two-DoF arms installed on a rotatable platform. Furthermore, characteristics of fast movement rhythm can make the dual-arm robot work quickly in limited space.

Kinematical modeling
Constructing the kinematic model of robot is the prerequisite of trajectory planning and control. Therefore, based on the D-H coordinate systems, a kinematic model of the robot is established with both forward and inverse solutions given to it. Only considering the single side of the robot, the schematic diagram of a single arm of the robot is shown in Fig. 2, and the D-H parameters of links are shown in Table 1.
On the basis of the transformation from the coordinate system i to i-1, the transformation matrix between the coordinate of manipulator and the base coordinate system is computed as in (1).

     
where Through combining the transformation matrix of robot, inverse kinematic solution is given as in (2).
The forward and inverse kinematic model can be extended to a dual-arm system via replacing θ 3 , d 2 with (1) and (2), respectively.

Workspace analysis and path planning
To ensure the robot can reach a target point in its workspace, this paper carries on the kinematics simulation and path planning in the MATLAB environment. Simulation of the single arm of the robot is conducted firstly, and then be extended to a dual-arm system. Based on the kinematics model and combining (2), the range of motion in vertical direction is -120∼820mm. The workspace in x-y plane is a circular ring that the external diameter is 755mm and the inner diameter is 563.5 mm [12].
As shown in Fig. 3, the task of dual-arm robot can be divided into four main steps: 1) The joint arm 1-2 moves to bottom of molding line (position 1) by controlling rotating platform.
2) The joint arm 1-1 moves down to locate vacuum chuck and grab the arranged ceramic tiles.
3) Put the arranged ceramic tiles in the top of packaging line (position 2) through controlling the joint arm 1-1 and joint arm 1-2.
4) When the joint arm 1-2 is stacked ceramic tiles, the joint arm 2-2 moves to bottom of molding line to grab ceramic tiles by moving down the joint arm 1-2. And two mechanical arms can be control independently to reduce the work rhythm of robot. Given multiple restrictions of workspace and operating points, working path of the dual-arm robot is gained by setting spatial coordinates points of the trajectory. As shown in left side of Fig. 4, the path is the full course of grabbing and stacking ceramic tiles. Average operation time of each arm is 6s, thus the dualarm robot can complete grabbing and stacking 20 times per minute. Since there are two vacuum chucks in the end of joint arm 1-2 and joint arm 2-2, the dual-arm robot can stack 40 layers of ceramic tiles per minute.

Dynamics modeling
This paper studies the dynamic model for the underactuated dual-arm robot, and verifies the applicability of the control strategy. In the absence of friction and disturbances, the dynamic model of the robot is established based on Lagrangian method. The equations of motion are simplified as (3).
where τ is a 5×1 joint's torque matrix, q is the joint variable matrix: where M=m 2 +m 3 +m 4 +m 5 . m 1 , m 2 and m 3 represent the mass of link 2 (shore with actuator), link 3 (arm 1-1/arm 2-1) and link 4 (arm 1-2/arm 2-2), respectively. m 4 represent the mass of actuator in the end of link 3. m 5 represent the mass of gripper in the end of link 4. J (q) is a 6×5 Jacobian matrix in this paper. f is an external force matrix. M(q) is a 5×5 inertia matrix such that its elements m ij satisfy (6). C(q, q  ) is a suitable 5×5 matrix such that its elements c ij satisfy (7). m i and a i represent mass and length of links (1 ≤i≤ 5).
where other parameters are 0.
As shown in Fig. 4, the maximum output speed of joint 1 is 225°/s (under 300°/s).The maximum output torque of joint 1 is under 60N•m which satisfies the performance index requirements. The torque is close to zero during the uniform motion, and it accords with the dynamic characteristics of uniform acceleration and uniform deceleration. The driving force of link 2 and link 2' are used to compensate for gravity and to realize motion control. Both the maximum output force of them are 135.5N. Above all, the experiment results with the inverse dynamic simulation prove that the correctness and efficiency of the approach.

Cartesian admittance controller
In practical applications, there could be some restrictions because the robot easily occurs collision with production lines when programming teaching. Among the researches of human-robot interaction, direct teaching is one of the intuitive teaching methods [2]. When the operator wants to teach the manipulator to follow the working path, he can move the end-effector of manipulator to a desired position or path under the workspace restriction. In this method, the Cartesian admittance control is adopted which is introduced to get different motions of the end-effector by changing admittance parameters. Admittance Control provides a high level of accuracy in non-contact tasks and results in good performance with low stiffness environments relative to impedance control [7]. In an admittance control algorithm, the force applied by the operator is sensed by the wrist load cell [13]. Considering one side arm of the robot, the responds with the end-effector motion satisfying relationship as in (8).  (9). The fourth order Runge-Kutta method is used to resolve differential equation and V ref can be obtained via differential method.
In inverse differential kinematics, transform from the reference velocity to reference joint velocity is implementing via inverse Jacobian matrix. Defined: where ref q  is reference joint velocity, † J is generalized inverse of Jacobian matrix.
As in (11), separating the Jacobian matrix into two parts: the translation part J T and the rotation part J R , each of them is a 3×3 matrix.
This work adopts the method described in [8] that adds a restriction to realize constant orientation during end-effector's translation. The restriction is represented from the null space projection, and (12) is used to rewrite matrix JT. This paper only considers the translational motion in Cartesian space, and (10) could be rewritten as (13). † (I ) For the practical applications, the control variables are discretized firstly, and the desired joint velocity can be solved as (14) by using the Euler's integration method.
where T is the system cycle period with its value is 1ms in this work.

Experimental setup
After discretization and programmed in robot controller, motion control of the robot is realized via position control and speed control based on the combination of software and hardware. In this paper, kinematics, dynamics, workspace analysis and trajectory planning of the robot are realized by using a real-time controller. Robot controller is designed based on embedded controller CX5130-0125 (Beckhoff Automation Inc., GER) with an Intel dual-core 1.75GHz processor, under the TwinCAT3 software environment. Function modules can be established according to requirements of the control algorithms.  A force/torque sensor (Delta IP60, ATI Industrial Automation Inc.) is fixed to the end-effector of manipulator to measure the teaching force, as shown in Fig. 5. Controller integrates network force/torque sensor system via a Real-Time EtherNet/IP interface. Output rate of the force/torque sensor up to 7000 Hz over Ethernet. In order to meet the high-efficiency and realtime requirements, the communication between the controller and the servo system is designed based on the EtherCAT protocol [14].
Based on above conditions, the admittance control time loop including the algorithm implementation, data acquisition and control signal output is set to 1ms. The system has been designed to have an extremely low latency to ensure its accuracy and stability.

Experiments and discussion
In this section, experiments are conducted with the dualarm robot for evaluating the proposed controller. During the trial, joint kinematic variables and control variables are collected through the TwinCAT ScopeView.
To validate the admittance control algorithm mentioned above, interaction experiment for direct teaching are implemented. Based on admittance control algorithm, operator can move the end-effector of manipulator to a desired position intuitively. The gains in (9) are set as M d =I, D d =50I. Fig. 6 presents the process that an operator applies a force to guide the robot in x-y plane.   Fig. 7(a) shows the relations between teaching force and joint velocity in z direction when operator applies a force along the z axis of the Cartesian frame of endeffector. Significantly, the time delay from applying force to resulting in velocity response is imperceptible. The contact force sensed by force/torque sensor is close to zero without teaching force, end-effector move smoothly while exerting a force upon it. It can be seen that there is no oscillation when applied the continuous force. However, when the joints work under a low-speed, there is a small vibration, which is due to the synthesis of sensor error, time delay and control error.   In the second experiment, inverse kinematic-based control methods and interpolation algorithm are evaluated. Interpolate algorithms based on robot controller are two-step process. The rough interpolation is designed according to the requirements of trajectory. For example, this project adopts the trapezoidal interpolation for Point-to-Point motion, and adopts the quintic polynomial interpolation for continuous track motion. Then, the fine interpolate is completed by the controller in real time.
The curves of joint motion and joint velocity of the robot along the work path are depicted in Fig. 8. At first, the joint arm 1 moves to the top of molding line (position 1), then moves down to hold tiles. Finally, joint arm 1 stacks the tiles in the top of packaging line (position 2). At the same time, the joint arm 2 is stacking and holding respectively. Fig. 8(a) shows that the movement of joints have negligible vibration and precise positioning. Obviously, the rotation angles and displacement of joints shown in Fig. 8(a) are very close to the expected path in left side of Fig. 4. Therefore, the experimental results are in agreement with the simulation curve. The results indicate that the robot can track the trajectory steadily, furthermore, the angle and velocity of each joint vary with time uniformly and smoothly.
Over all, motion control algorithm of the robot is designed rationally because of its precise control for motion track and moving velocity. The experiment reveals that the admittance control algorithm works well for direct teaching while achieving force tracking. However, the movements tracing by direct teaching can hardly satisfy an expected performance under some constraints. In practice, operator can directly teach the manipulator to a series of target points and implement path planning to find trajectories between target points in order.

Conclusions
In this work, a simple structured and high speed dualarm robot is designed for the stacking of ceramic tiles in limited space. The control strategy is built on robot kinematics and admittance control, then realized in a high speed robot controller with real-time bus EtherCAT. To implement basic functions of robotics control, kinematics, dynamics, workspace analysis and trajectory planning of the robot are discussed. Based on Cartesian admittance control algorithm, direct teaching for the force-free control is presented.
The system has been designed to have an extremely low latency which allows it to get a stable, accurate tracking effect. The performance of the proposed method has been examined through the numerical simulation and practical testing. At last, the experimental results on the newly designed robot have shown the validity of the proposed control system. Our work would provide certain guiding significance to the practical production. Future work will concentrate on vibration suppression and robustness test.