Algorithm to obtain inverse kinematics matrix from the 3D curve and to apply to glue shoe sole

: Nowadays, manipulator is widely used in industrial applications. The trajectories of manipulator are more and more complicated. In order to do good tracking performance, the end effector position and orientation have to be determined. This paper describes a method to determine position and orientation of manipulator’s end effector base on a reference path. This method will be applied for manipulator 6 DOF to glue shoe sole. Firstly, assume the reference path is arbitrary curve, the path was then discrete to become multi-point. Secondly, the roll – pitch – yaw vectors of the end effector will be determined at each point. Finally, Euler angles and interpolation method in 3D space will be applied to determine inverse kinematics matrix of manipulator for each point on the reference path. In addition, this paper also gives an example of reference path of shoe sole to apply the presented method. To verify the tracking performance of manipulator and reference path, a PID controller was designed for simulation. The result of simulation proved the correction of the algorithm.


INTRODUCTION
Most of manipulators were controlled by using teach pendant. Controller of manipulator is integrated available programs. Users only use teach pendant to control manipulator for their application. Mostly, users can not rewrite or edit the program of manipulator's controller. After the teaching is completed, the manipulator will repeat the process taught. So, it is very difficult for manipulator closely tracking a reference path. The errors of tracking process entirely depend on the skill and experience of the users. Therefore, teach pendant is used to control manipulator when the accuracy of tracking performance is not interested. reference path is complex curve without precise mathematical equation. In addition, high accuracy of tracking performance is required. Furthermore, the end effector's orientation continuously changes during tracking process.
So, it is a cause cannot use teach pendant to control manipulator's trajectory. To solve this problem, the automatic controller must be applied. But, this method is not easy to work, the biggest problem is how to get inverse kinematics matrix of manipulator from the reference path. It means that the user must calculate the joint variables of manipulator. These joint variables must satisfy both position and orientation of manipulator during tracking process. proposed a method to identify and analyze a technique to be used in a 3D virtual robotic simulator that executes smooth and continuous movement. Thomas Horsch et al. [7] described an algorithm for interpolation of position by rational spline motion, The whole spline scheme possesses some special features which make it a suitable tool for the control of industrial robots.
S.D Voliotics et al. [8] presented an algorithm of trajectory planning for wrist partitioned robotic manipulator. The path is generated as a sequence of elementary motions. Each new position is defined as a function of the previous one. After having derived a position, the orientation is planned using an optimal process.
The mentioned papers either give the solution for general inverse kinematics of manipulator or interpolate to get the position of manipulator's working path. However, they do not clearly mention how to take orientation of manipulator when the orientation continuously changes during tracking process.
This paper proposes a method to obtain inverse kinematics matrix from 3D curve based on Euler angles and interpolation method. This paper clearly solved inverse kinematics problem and to show a method to take the orientation of manipulator when the orientation continuously changes during tracking process. This method will be applied for manipulator 6 DOF to glue shoe sole.

ALGORITHM
A block diagram of the proposed algorithm is shown in   point.
At each point on the interpolated curve, the Euler angles [9] can be obtained as follows.
Substitute Eqs. (1)  there is a set of angles that satisfies Eq.(12). In a similar way, the interpolated curve is done with three-consecutive points on entirely reference path

Gluing path
In this part, the proposed algorithm will be applied for 6-DOF manipulator to glue shoe sole. Assume, the curve of shoe sole (gluing path) (Fig.4) is divided into multipoint with isometric sampling distance.

Robot modeling
Let C ( ) and are the wrist point and the end effector's position of manipulator respectively. The wrist point [10] will be calculated as follows, From the manipulator configuration (Fig.6), the joint variables were obtained as follow, where, is distance from C to origin O of coordinate.
Apply the law of cosines to obtain (14) where, From Eq.(12) the end effector's orientation is calculated via wrist's orientation matrix [11] (16) where, Say in other words: where, are joint variables

DISCUSSION
In the traditional method of trajectory control, the manipulator's orientation is a given matrix.
Manipulator will move on trajectory with constant orientation, for instance the final inverse kinematics matrix of trajectory as follows, The orientation: The position: It is easy to realize that the orientation does not change when the end effector moves on trajectory. This method is only applied in cases the orientation of the end effector is neglected.
In the previous method of trajectory control, the reference path was divided to multi-line which the length is not equal. For instance, there is a curve, which is divided into 6 lines (Fig.7)  Table 1.

PID controller design for simulation
The transfer function of PID controller with the filter is then where a firstorder filter is used.

Controller
Proportional gain, Integral gain and derivative gain Time constant A block diagram of PID controller with first order filter is shown in Fig.8. A block diagram of control system is shown in Fig.9 , and it was calculated by Eqs. (12 -18). The results of simulation are presented in section 4.2

Simulation result
The results of simulation include position's tracking performance and orientation's tracking Figure 8. A block diagram of PID controller with firstorder filter   From the simulation result, the manipulator can track arbitrary curve both position and varied orientation with acceptable error. So, the proposed algorithm can be used to calculate inverse kinematics matrix for manipulator from the reference curve.

CONCLUSIONS
This paper introduces an algorithm to obtain the inverse kinematics matrix of manipulator from the 3D curve based on Euler angles and interpolation. The good tracking performance of manipulator's end effector and reference path proved potential of algorithm.