Abstract
In order to ensure the steady ability of the LED wafer transporting robot, a high order polynomial interpolation method is proposed to plan the motion process of the LED wafer transporting robot. According to the LED wafer transporting robot which is fast and has no vibration, fifth-order polynomial is applied to complete the robot’s motion planning. A new subsection search method is proposed to optimize the transporting robot’s acceleration. Optimal planning curve is achieved by the subsection searching. Extended Kalman filter algorithm and PID algorithm are employed to follow the tracks of planned path. MATLAB simulation and experiment confirm the validity and efficiency of the proposed method.
1. Introduction
The robot motion planning is one of the most important problems for the robot control. Motion planning includes path planning and motion control. Path planning is to search an optimal trajectory of the path from the beginning point to the finishing point in the robot motion space. Reference [1] uses an ant colony optimization algorithm to realize path planning. Reference [2] proposes coordinated trajectory planning methods of two typical applications, which can assure the applications’ stabilization. Reference [3] proposes autonomous motion control approaches to control dual-arm space robot for target capturing. Reference [4] proposes quantum-behaved particle swarm optimization (QPSO) algorithm to plan the robot path, and author uses the probability theory to study the relationship with the parameters and convergence of mobile robot path planning, and at last author proposed an improved trajectory planning method. Reference [5] points out that the basic problem of the path planning is the common model expression and path optimization strategy. Common model expression methods include visibility graph, free space method, and grid method. Optimal path search problem is then converted to search the shortest route from a beginning point to the target point via the visible lines. By far, most robots employ trapezoidal speed curves for their motion planning.
Some problems were discovered by research. The classical planning methods have some distinguishing feature. First, when the acceleration or the velocity is fixed, the acceleration and the velocity must be set to a low value to ensure that they do not exceed the constraints during the whole process which is impossible to optimize acceleration and velocity at some points in whole moving process. Second, sudden change of acceleration can cause system oscillation. System oscillation can be reduced by planning the motion acceleration. When the robot moves according to a control method based on its motion model, sudden change of the acceleration will certainly cause the system oscillation. The principal purpose of motion planning is to discover a reasonable polynomial function or other functions to conduct the interpolation, so that the robot motion can be smooth and stable with vibration within the acceptable range. And the consumed time from the beginning point of the motion to the finishing point is as few as possible.
In order to solve the problems on how to realize large range, high speed, and high accuracy trajectory tracking, robot dynamic real-time control method is raised which has two problems. The first is how to keep the system stable. Advanced control method should be studied and applied to robot real-time control, so that the tracking error can diminish to possible range as quickly as possible. The second is how to diminish disturbance and how to reduce the influence of the disturbance to the accuracy of the tracking. If the precise motion model of the robot can be achieved and the disturbance signal can be eliminated, then controller designed with linear control theory can solve the two problems. But the precise and complete motion model of the robot almost cannot be constructed, because of the error in measurement and modeling, the changing of the load, and disturbance from the environment [5–7]. So when the robot motion model is constructed, some reasonable approximations had to be made and some unimportant uncertainties might be ignored. In the field of industrial control, the PID control algorithm is one of the most important control algorithms. It plays a vital role in the industrial production process. As far as control field is concerned, PID control method has the dominated position in the field for many years. Although much progress has been made in model based mathematical control science, PID control method has the significant impact on industry. In recent years, some scholars begin to attempt some new control method to replace the PID method [8–10]. After many years’ unremitting efforts, using the Kalman filter tracking control is a trend in the aviation field. But Kalman filter’s application is less in any other areas [11–14].
In the process of transporting the LED wafer, we need to transport it as fast as possible. Moreover, the vibration of the fast transporting process of the robot must be as small as possible. In this paper, two fifth-order polynomial interpolation functions and a first-order polynomial interpolation function are used to plan the motion trajectory of the fast transporting robot. In addition, a new method is proposed to minimize the maximum acceleration value in the acceleration stage and deceleration stage to reduce the oscillation. PID algorithm and extended Kalman filter algorithm are proposed to track control the movement of the fast transporting robot.
2. Motion Planning Theory
The fundamental task of the robot motion trajectory planning is to select reasonable polynomial function or other linear functions to accomplish interpolation operation task [15–18]. It can make robot movement smooth, steady ability, and keep robot movement error within certain range. In the process of the robot movement, the robot position at the beginning point is known, and the robot position at the finishing point can be achieved by using the inverse kinematics. Thus, the description of the motion trajectory can be represented by a smooth interpolation function, which can describe robot position from the beginning point to the finishing point. At the time , is the beginning point of robot position . At the time , is the finishing point of robot position . In order to realize the smooth and steady movement of transporting robot, trajectory at least needs to meet four limit conditions:
The above four conditions can define a unique third-order polynomial equation:
Equation (2) is position of the robot. The first derivative of (2) is the speed of the robot:
Equation (1) was substituted into (2) and (3); the following equations can be obtained:
Equation (4) can be written into matrix form:
The following result can be achieved by calculating (5):
A unique third-order polynomial equation can be determined by (6). Therefore, if the beginning position, beginning speed, finishing position, and finishing speed are known, using third-order polynomial interpolation method can acquire a complete motion trajectory equation. When the robot system’s acceleration has to be limited, a fifth-order polynomial interpolation method will be needed to plan the motion trajectory of the robot movement process. Equation of the robot’s position is expressed in the following form:
First-order derivative of (7) is the robot’s speed of the motion:
Second-order derivative of (7) is the robot’s acceleration of the motion:
The moving process of the fast transporting robot requires small vibration. According to those characteristics, the whole motion trajectory is divided into three stages: accelerating stage, uniform stage, and decelerating stage. The acceleration and time are all important in the accelerating stage and decelerating stage, so fifth-order polynomial interpolation is employed to plan accelerating stage and decelerating stage. One order polynomial interpolation is used to plan uniform stage. The accelerating stage uses the following equation to plan:
The uniform stage uses the following equation to plan:
The decelerating stage uses the following equation to plan:
Four moments are critical for the whole trajectory of motion planning, which are the beginning time of the accelerating stage , the finishing time of the accelerating stage , the beginning time of the decelerating stage , and the finishing time of the decelerating stage , and the corresponding positions of the robot are , , , and . In order to optimize the maximum acceleration value both in the accelerating stage and in the decelerating stage, , , , and must be optimized. Hence, subsection searching method is used to find the optimum , , , and .
Remark 1. For the motion equation, is variable of equation and is coefficient of equation. The beginning point and end point of are selected to acquire the value of coefficient .
3. Acceleration Optimization
The movement trajectory can be planned by interpolation through some point in the path. The whole trajectory can be divided into a number of segments by those special points. If robot stayed at some point of the path for a while, in which beginning velocity and the finishing velocity are zero, therefore polynomial interpolation method can be directly used. If it does not stop at a point in the process of movement, inverse kinematics solution can be used to determine polynomial interpolation function and connect every point of path smoothly. Fifth-order polynomial interpolation method is used to plan the trajectory of the movement process of the fast transporting robot during the accelerating stage. Its beginning position , beginning velocity , and beginning acceleration are as follows:
At the end of the acceleration stage , the position , velocity , and acceleration are as follows:
Equations (13) and (14) were substituted into (10); the following equation can be obtained:
Remark 2. In (15), is known; , , , and are unknown. , , , and can be obtained by and .
Equation (15) can be rewritten as
In (16) is the robot velocity of the uniform stage, which can be achieved by substituting , , , and into (11):The acceleration equation of the accelerating stage is
Subsection searching method is used to find the optimal , , , and , which can minimize the maximum acceleration value of the nonlinear high order polynomial of the accelerating stage. Subsection searching method is proposed based on branch and bound method of optimization theory. Branch and bound method needs two stages of operations: The first is branch, which divides the solutions into several nonintersect solution sets, according to certain rules. The second is bound, which selects an appropriate algorithm to compute the bound of the subsection which will be conducted again and again; thus the solution set will become smaller and smaller, and at last, an accurate solution will be achieved.
Figure 1 is the process of searching using subsection searching method. The proposed subsection searching method in this paper searching process includes the following stages. The first is to choose a random point in the solution set as the starting searching point. The second is to begin subsection. In the figure, is last computing maximum acceleration value based on given subset and target function and is this time computing maximum acceleration value. Subsection direction depends on the results of comparison and value. Search speed depends on . In order to improve the computing speed, can be chosen to increase or decrease. The step length can choose 0.1, 0.01, or 0.001. After repeated computing and comparison many times, optimal can be obtained. For obtaining optimal , the same method can be applied and then optimal and can be obtained. Substituting , , , and into (16) and (17), , , , , , and can be obtained.
Remark 3. By searching the proportion of the time of accelerating stage, decelerating stage and uniform stage motion curve can be optimized.
4. System Modeling and Control
4.1. Modeling
Servomotor is used to control the motion of the fast wafer transporting robot. Servomotor’s job is to transfer the input electric power into the robot system’s mechanical energy [19, 20]. The mechanical structure of the transporting robot is shown in Figure 2.
In Figure 2, 1 is installer. 2 is bearing A. 3 is lead screw A. 4 is servomotor B. 5 is encoder B. 6 is encoder A. 7 is servomotor A. 8 is lead screw B. 9 is workbench. 10 is transporting arm. 11 is bearing B.
In Figure 3, 1 is transporting arm. 2 is vacuum pad. 3 is wafer. Vacuum pads grab the wafer and transport them from source position to target position. In the transporting process, movement is smooth and has no vibration.
In servomotor, the rotor voltage and the rotor current are induced in the rotor circuit. Then the rotor current and stator magnetic flux interact to produce electromagnetic torque . Its equation is as follows:where is counter electromotive force (EMF), , is EMF constant, is the inductance parameter, and is the rotor circuit resistance value.
Electromagnetic torque equation iswhere is servomotor torque coefficient and is the electromagnetic torque produced by servomotor.
Servomotor torque balance equation iswhere is motor shaft sticky friction coefficient, is motor shaft rotary inertia, and is total load torque. Remove the middle variable of (19), (20), and (21); the following motor differential equation (22) can be obtainedIn (22), the inductance is very small, which can be ignored, so (22) is simplified as follows:where If , (23) becomesApplying Laplace transform for (25), the following equation can be obtained: Furthermore, transfer function can be obtained from voltage and angular displacement :
4.2. Kalman Filter Tracking Control and PID Control
The extended Kalman filter is used to track the position and velocity of system. Transporting robot moved according to the given direction and speed. Sensors are used to measure the distance and azimuth. Considering the noise of the system motion process, at time , the system speed component is as follows: In the equation, is noise disturbance. According to the motion equations, the following location equations in time are as follows: In the equation, is the interval between samples. In the discrete model of the equations of motion, the system will move according to the speed of a moment ago and then suddenly change at the next moment. Now, the signal vector by the choice is made of the position and velocity components. The equation is as follows:Equations (28), (29), and (30) are replaced by The observation equation of the system is In the equation, is a function. In order to estimate the signal vector, the extended Kalman filter is applied; we now need to determine :
The Jacobian matrix can be obtained through seeking for the derivative of the observation equation.
Remark 4. Through Jacobian matrix, Kalman filter can estimate ; therefore, it can track the motion trajectory, and it can obtain the better tracking effect.
PID controller is as follows: where , is the input, and is the output. , , and are PID parameters. is control variable. Furthermore, (34) can be written as follows:where and .
The discrete equation is as follows:
5. Simulation and Experiment
The fast transporting process of the LED wafer transporting robot was simulated by applying the proposed method. Firstly, the motion trajectory is divided into three segments, which are acceleration stage, uniform stage, and deceleration stage. The parameters are set as follows: s, mm, and . Then, we begin to search the extreme value point using the multisegments searching method. After this point is found, two fifth-order polynomials and a first-order polynomial are used to plan system’s trajectory. At last, we obtain the motion trajectory with the minimum acceleration when .
Figures 4 and 5 show the simulation results of the motion planning using MATLAB. As shown in these figures, all the curves of position, velocity, and acceleration have no singular point. The changes of the acceleration can be seen from Figures 4 and 5. Maximum acceleration is 270 mm/s2 before optimization, and then it becomes 200 mm/s2 after optimization. Maximum acceleration reduces 26%. Thus, this planning trajectory is more reasonable.
(a) Acceleration curve
(b) Velocity curve
(c) Position curve
(a) Acceleration curve
(b) Velocity curve
(c) Position curve
The mechanical parameters of the transporting robot are shown in Table 1. According to these parameters, we can obtain and . The transfer function is
The movement curves of the LED wafer transporting robot are shown in Figure 6, where the Kalman filter and PID controller are used to track the movement curve. In Figure 6(a), is planned position curve, is used PID controller to track output position curve, and is used Kalman filter to track position curve. In Figure 6(b), is planned velocity curve, is used PID controller to track output velocity curve, and is used Kalman filter to track output velocity curve. In Figure 6(c), is planned acceleration curve, is PID used controller to track output acceleration curve, and is used Kalman filter to track output acceleration curve. From the above curves, we can see that the motion of the transporting robot agrees more similarly with the planned trajectory. Position curve, velocity curve, and acceleration curve are all very smooth, and acceleration is very small. The tracking result of the Kalman filter is better than the PID controller.
(a) Position curve of the transporting robot
(b) Velocity curve of the transporting robot
(c) Acceleration curve of the transporting robot
6. Conclusion
The LED wafer transporting robot should open as fast as possible with small vibration. This paper proposes the fifth-order polynomial to plan the motion trajectory for the fast transporting process of the transporting robot. In order to minimize the maximum acceleration value during the motion, subsection searching method is designed to select the fifth-order polynomial coefficient. The optimized fifth-order polynomial is simulated in MATLAB. At last, PID method and extended Kalman filter are used to track the planned curve. The simulation and experiment result show that the motion trajectory, velocity, and acceleration are smooth in the whole process. And acceleration is small, which satisfies the design requirements. Simulation and experiment show that the proposed subsection searching method for planning motion trajectory for the LED wafer transporting robot is very effective. The extended Kalman filter is applied to track motion trajectory of planning. Simulation and experiment show that the track effect of the extended Kalman filter is better than PID.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This work was supported in part by the Science and Technology Innovation Project of Jinan under Grant no. 201309, National Natural Science Foundation of China under Grant no. 61403221, and Open Foundation of Key Laboratory of Pulp and Paper Science and Technology of Ministry of Education of China under Grant nos. KF201419 and 08031347.