Control Algorithm Trajectory Planning for Dual Cooperative Manipulators with Experimental Verification

. In order to achieve cooperation of multi-robots simultaneously, this paper presents an approach of Trajectory planning for two decoupled robots with six degrees of freedom (DOF) each to execute packing task. The Denavit-Hartenberg (D-H) representation is used to model robot links and solve the transformation matrices of each joint. The inverse kinematics solution using for a 6-DOF Robotic arm is presented, for given initial and final configurations of the robots, the robots are placed so that their workspaces overlap. An algorithm successfully finds a set of configurations to ensure, collision free transition from start to end configuration. Simulation and experiments based on 6-dof robot are carried out and the results verified the effectiveness of the proposed algorithm.


Introduction
Dual arm manipulators demand has increased with the public expecting robots that mimic human's behavior.therefore, studying dual-arm manipulators is a natural avenue of researches [1].variety of robot platforms used for dual arm manipulation, while some researches are carried out on systems built by simply placing two single arm manipulators for sharing same workspace [2].
The difficulty in mechanical analysis for dual arms comes mainly from the closed chain system, which is formed during the manipulation of the objects, and which is much more kinematically and dynamically complex than the typical serial manipulator configuration [3].
In general, control of multiple robot cooperation systems, the master-slave method is still the state of art for industrial robots [4].The Master-Slave control strategy has to identify one robot as Master, the other as Slave.
A number of research works have been presented relating to the modelling of kinematics.Shi et al. have proposed the solution for forward kinematic problem of 6 DOF robot [5].comprehensive survey can be found in [6].
In [7] analyzed the problem of path planning for two cooperating arm robots carrying an object between two given configurations, in a setup which includes obstacles.the method proposed extends the randomized potential field planning techniques introduced in [8].
The two keys contributions of this work are: (1) Designing control algorithm of the robots to conduct the prescribed task.(2) Cooperative robot manipulators system performing the assembly task.Suggested system is including two arm robots.Target of these robots is cooperating for Drug Packing.This paper is structured as follows: firstly the proposed robotic platform and developed kinematic models introduced.Trajectory planning approach was discussed, then explains the design details of the electronic hardware.Finally Experimental results for two Cooperated robots are presented and concluded the work.

Coordinate frames assignment
The proposed system is containing two 6-DOF robots manipulators.This Robot has five rotational joints and a moving gripper. Figure 1 illustrates the graphical view of all the joints of the presented robot.Joint 1 represents the base and its axis of motion z 1 .This joint provides a rotational θ 1 angular motion around z 1 axis in x 1 y 1 plane.Joint 2 is identified as the Upper Arm and its axis is perpendicular to Joint 1 axis.It provides a rotational θ 2 angular motion around z 2 axis in x 2 y 2 plane.z 3 axes of Joint 3 and Joint 4 (Wrist) are parallel to Joint 2 z-axis; they provide θ 3 and θ 4 angular motions in x 3 y 3 and x 4 y 4 planes respectively.

Denavit-Hartenberg (D-H) parameters
The Denavit-Hartenberg analysis is one of the most used.depending on each mechanism.Based on the analysis, once one coordinate transformation between two frames is defined, D-H parameters for this robot defined for the assigned frames in Table 1.

Table 1. Link parameters of slave manipulators (D-H parameters)
Based on these parameters, the transformation matrix includes the overall rotation and translation of tool frame {B} with respect to base frame {A}.
Assume that, i a is the length of Manipulator's link i, i T is the angle of joint i, i D is the angle between link i and link i-1, d i the distance between link i and link i-1 , where i =1,2,…,6.i a , i D , i d are all known and i T is proposed to be obtained In this paper, the link parameters of robot are given in Table 1 above.
According to calculated 2 2 O P T , suppose that:

Analysis and calculation of dual robots
In space geometry, using a unique 4×4 homogeneous transformation matrix B  A P can describe the motion from coordinate system ^Β of Point B to coordinate system ^Ὰ of Point A, i.e: where T R is translation matrix.If a series of motions appear, composite transformation will be a good choice to describe.
Assume that, ^1 O ( ^2 O ) is the base coordinate system of master (slave) manipulator and ^1 P ^2 P is the end coordinate system of master (slave) manipulator as shown in Fig. 2. Then 1 )is a transformation matrix from the end coordinate system to the base coordinate system of master (slave) manipulator.relative relation of the extremity pose between master manipulator and slave manipulator during their movements is constant.It is particular case of dual manipulators collaborative movements and can be described as:

4.1Transformation from initial to final point
In this experiment work Master Manipulator was required to move from initial position A 1 [0,0,0] to a given position B 1 [ , , 2 ]to pick up the Bottle, then to position C 1 [ , 5 , ] to position D 1 [250, 0, ] in the Cooperated area and finally to initial position again A 1 [0, 0, 0] in 12 Seconds.And the Slave Manipulator was required to move from initial position A 2 [0,0,0] to a given position B 2 [ , , 2 ] to pick up the Box, then to position C 2 [ , 250, 100] to position D 2 [-250, 0, 00] in the Cooperated area then to position E 2 [-210, 210, 00] to locate it in the Delivery area and finally to initial position again A 2 [0, 0, 0] in 14 Seconds.Figure 3 illustrates the top view of packing process task.The motion was separated to four segments for Master Manipulator and five segments for Slave Manipulator, defined by the time period .Figure 4-5 illustrate the angle Values of the Joints of Master and Slave Manipulator in whole motion segments, the initial position (0,0,0) will be used as a reference to all joint angles of the robots.

Experimental results and discussion
Experiments of Dual arm Robots in Cooperating task motion are carried out.As shown in Figure 6, Master robot picks a bottle while Slave carries a Drug box.The slave robot tracks the trajectories on the box while the master transports the bottle.sensory feedback used to add an advantage of closed loop controller for the control system.
Execute the program and perform operation, the whole process will be as follows: Master and slave Manipulators were moved from initial to final positions as discussed previously.whole real sequence process of Dual Manipulators cooperation for drug Packing shown in Figure 6. (1) (2) (3) (4) (5) (6) (7) (8) Figure 6.Cooperation process task of dual manipulators.
In this experiment, the control system was set up and the task of dual manipulators cooperating for drug packing was performed using the method put forward before.By the above figures, it is clear that the two robots can move in a Cooperative motion Therefore, the results verified the effectiveness of the proposed algorithm.

Conclusion
The aims of this paper were to establish the theoretical foundation for automated cooperative trajectory planning of Dual arm robots.Motion constraints between cooperative master and slave robots are the basis for this method.Denavit-Hartenberg (D-H) representation is used to model robot links and solve the transformation matrices of each joint.The inverse kinematics solution for a 6-DOF Robotic arm was presented.the proposed algorithm was successfully implemented on the packing Cooperating task.this work was motivated bearing in mind that the two arm robots could be controlled by one controller simultaneously in centralized fashion that would reduce the cost of production effectively.
Figure 2. The simplified model of dual manipulators cooperating at 0 t .
Trajectory planning for robotic manipulators is the process of creating trajectories free of collisions allowing the manipulators to perform the required task.the trajectory calculation formulas of Dual Manipulators under two different collaborative movements were put forward in literature (10).trajectory planning of Dual Manipulators is transformed into the teaching trajectory of single Manipulator through calculation.However, the algorithm given in the literature (10) is complicated.Different formulas are bound to be used in different circumstances.