Optimization on Trajectory of Stanford Manipulator based on Genetic Algorithm

. The optimization of robot manipulator’s trajectory has become a hot topic in academic and industrial fields. In this paper, a method for minimizing the moving distance of robot manipulators is presented. The Stanford Manipulator is used as the research object and the inverse kinematics model is established with Denavit-Hartenberg method. Base on the initial posture matrix, the inverse kinematics model is used to find the initial state of each joint. In accordance with the given beginning moment, cubic polynomial interpolation is applied to each joint variable and the positive kinematic model is used to calculate the moving distance of end effector. Genetic algorithm is used to optimize the sequential order of each joint and the time difference between different starting time of joints. Numerical applications involving a Stanford manipulator are presented.


Introduction
As modern industry advances towards intelligence and digitalization, the applications of industrial robots is becoming increasingly wider and deeper [1]. Robot manipulator is one of the most important devices among various industrial robots, which covers a diverse array of technologies from mechanical engineering, mathematical, to control theory, electronic engineering [2]. The optimization of trajectory plays a key role in the fulfillment of engineering tasks of robot manipulators, which has become a hot issue for academic and industrial research.
Atsushi et.al, who focused on the local navigation problem taking into consideration the dynamics of the robot, proposed a new technique with obstacle avoidance, called adaptive navigation. The advantages of the proposed navigation scheme are that less local information is required than in some other techniques and the navigation law is simpler and more flexible than ones using the artificial potential field methods [3]. This model has some limitations in expressing the relative motions between the end effector and the robot.
Ningyue et.al presented the ADRC controller in mobile manipulator system. A kind of dynamics decoupling control method is proposed to achieve synergy tracking between mobile platform and robotic arm. The effect of overall estimation and compensation ADRC control algorithm has on the disturbance of the model and manipulators tracking control strategy can be a good trajectory of the end-effector and mobile platforms separately controlled, and reduce the instability factor between the two coupling caused by the dynamics, reaching a higher position tracking accuracy [4]. In contrast to the previous research, this paper presented the structural relationship between the shape of manipulator and the robot body. They were not able to address the path planning problem in three-dimensional space, which is still a gap in actual industrial production. The robot mathematical model in three-dimensional space will be more complicated and contains more constrains.
Jia proposed a positive-inverse kinematics model for Stanford Manipulator, using cubic B-spline interpolation to achieve the trajectory planning of Stanford Manipulator joints [5]. But the article did not take into account the effect of the sequential order of the joint movements on the trajectory of the end effector.
Genetic algorithm (GA) was used by many researchers to solve the problem of robot path planning, which get the global optimal solution in a short time [6] [7]. Dong combined the analytic method and genetic algorithm, obtaining a kinematic inverse solution with a redundant degree of freedom and achieving the optimal track planning and obstacle avoidance. The method has the characteristics of small calculation and strong adaptability [6].
Above all, the Stanford Manipulator is used as the research object in this paper, and the inverse kinematics model is established by Denavit-Hartenberg method. According to the initial posture matrix, the inverse kinematics model is used to find the initial state of each joint. In accordance with the given beginning moment, cubic polynomial interpolation is applied to each joint variable and the positive kinematic model is used to calculate the end effector path. Genetic algorithm is then used to optimize the sequential order of each joint and the time difference between different starting time of joints, with an objective function of minimizing the path of end effector.
[ ] Model of Stanford Manipulator 0 shows that Stanford Manipulator consists of a Rotation Body, a Big Arm, an Extension Arm, Wrist and an End effector, which is made up of six joints (Five rotating joint and one locomotive joint). Each joint can express one degree of freedom. From bottom to top, the six kinematics parameters of Manipulator's joints are θ � , θ � , d � , θ � , θ � , θ � . Obviously, end effector's position and attitude is related to the structure of Manipulator' connecting rod and kinematics parameters of Manipulator's joints. The matrix whi describes the end effector's position and attitude in reference coordinate system is expressed as follow: (1) Where � ��, � �, � � is the end effector's attitude, � � is the end effector' position, � � , � � , � � is the end effector's coordinate in the datum reference coordinate system. In order to make sure the matrix of effector's position and attitude T, the coordinate systems need to be set up at every rod. The relative position and attitude between these coordinate systems can be described by homogeneous transformation.
In this passage, we set up six coordinate systems on the Stanford Manipulator, and describe their relationship by matrix. As usual, we define Matrix A as homogeneous transformation among a rod and its adjacent member. A tandem manipulator is connected by a series of rods with a drive joint. A manipulator with n degrees of freedom has n rods and n joints. The foundation rod is called rod 0. For Stanford Manipulator, it is not considered into the six rods. The rod 1 and rod 0 are connected by joint 1. The rod 2 and rod 1 are connected by joint 2, and so on. There is no joint at the end of rod 6.

Fig 2.
Coordinate transformation of adjacent connecting rod 0 shows the relationship of the adjacent coordinate system n-1and n. Firstly, the � ��� axis is rotated by � � angle around the � ��� axis. Secondly, the � ��� axis moves � � distance along the � ��� axis. Thirdly, the � ��� axis moves � � distance along the rotating � ��� axis, that is, � � axis. Finally, the � ��� axis is twisted by � � angle around the � � axis. As a result, the Matrix A is presented as follows, According to the structure of Stanford Manipulator, the link motion parameters is shown in 0.
Therefore, when the reference coordinate system is transformed to the base coordinate system, the Matrix of manipulator end effector's position and attitude can be represented as: To facilitate writing and reading, we denote ���� � � � � , ���� � � � � , forward kinematics model, in which the position and attitude matrix T can be obtained according to the joint parameters. We can also know that the position of end effector relative to base coordinate system � � , � � , � � only depends on the joint parameters θ � , θ � , d � .
In industrial applications, we often design the position of end effector � � , � � , � � and solve the corresponding joint parameters θ � , θ � , d � . This is called inverse kinematics. For Stanford Manipulator, the inverse kinematics analytical solution is as follows,

Motion trajectory planning
From Chapter 0 we can know that the parameters of manipulator's joints �θ � , θ � , d � � can determine the position of end effector �� � , � � , � � �. And the trajectory of end effector will be determined by the position �� � , � � , � � � at all moments. In trajectory planning, we solve the initial values and final values of joints' parameters by inverse kinematics according to the given corresponding initial position �� �� , � �� , � �� � and end position �� �� , � �� , � �� � . Besides, the time slot from the former to the latter � � should be specified according to the task requirements. Then, each joint parameter will be performed by polynomial interpolation operation to get smooth function θ���. Finally, we get position-time function ����, which can accurately indicate the end effectors trajectory, by positive kinematics algebra for the smooth function θ���. According to mechanical knowledge, manipulator's movement should be smooth, because uneven movement will aggravate the wear of mechanical parts and cause manipulator's vibration and shock. Hence, the described trajectory function must be continuous. Its first derivative (Velocity) and even the two derivative (Acceleration) should be continuous too.
In this passage, the parameters of manipulator's joints are processed with three polynomial interpolation. So there are four constraint conditions of the joints' parameters as follows, (24) The function that satisfies all of the above constraints is shown as follows, θ�t� � � � � � � � � � � � � � � � � � (25) The constraint is replaced by a function, and get undetermined coefficient as follows, Obviously, it is completely unnecessary to make all parameters of Manipulator's joints start changing at the same time. In the other words, there are many kinds of orders sequences between different parameter variations. With a result the trajectory of end effector will be also different. Suppose intermediate moment t � t � is inserted between starting time t � 0 and finishing time t � t � , a part of parameters of Manipulator's joints begin changing at t � 0, and the others begin changing at t � t � . All parameters stop at t � t � and the end effector reach the final position.
So there are serval constraint conditions of the joints' parameters which begin changing at t � t � as follows, When t � t � , When t � t � , the function that satisfies all of the above constraints as (25), The constraint conditions (22)-(24)change to (28)-(30) as follows, The undetermined coefficients are calculatedas follows, In this passage, the trajectory optimization objective is to find the middle moment between joint 2 and joint 3 to minimize the movement distance of end effector when the initial position �p �� , p �� , p �� � and end position �p �� , p �� , p �� � is given. Now, the concrete trajectory planning requirements for Stanford Manipulator is given as follows. The joint parameter of rotation body θ � will always start changing at t � � , but the joint parameter of big arm θ � and the joint parameter of extension arm d � will start changing at t � t � and t � t � ,respectively. The three joint parameters must stop at t � t � , at the same time the end effector must arrive the final position.
The solution is to get the best combination of middle moment t � , t � , which can make the distance of end effector from start position to final position the shortest.

The Proposed Genetic Algorithm
In this passage, genetic Algorithm(GA) is used for global search to find the shortest distance of end effector L and the corresponding middle moment � � , � � . Genetic Algorithm consists of the following functions, initialization parameters, fitness function, ranking strategy, selection, crossover and mutation probabilities. The main body of GA is shown in 0 initialization(pop_size,chromo_size); for i=1 To generation_size fitness(pop_size,chromo_size); rank(pop_size, chromo_size); selection(pop_size, chromo_size); crossover(pop_size,chromo_size,cross_rate); mutation(pop_size,chromo_size,mutate_rate) ; end for Initialization function is shown in 0. It should be noticed that the column length of population (pop) should be two times that of chromosome length (chromo_size), because there are two independent variables. pop(i, j) is equal to 0 or 1each with a fifty percent. In this case, individuals composed of arbitrary middle moment t � , t � is represented as a binary code whose length is equal to two times the length of chrom_size. Fitness function is used to calculate population fitness (fitness_value) as 0. Each individual should be divided into two parts and converted into decimal numbers. Then, middle moment t � , t � is based on the corresponding decimal numbers in the ��, t � � range. Upper limit t � is different from final joints stopping time t � . The wear and vibration will be intensified if the two numbers are too close. The first half of each solution is transformed into the moment when parameters of joint 2 begins changing t � , while the second half of each solution is transformed into the moment when parameters of joint 3 begins changing t � . In the end of this function, t � , t � is substituted into calculations to get the distance of end effector L. To facilitate the arrangement of fitness_value from small to large, fitness_value is defined to be equal to the reciprocal of L. if pop(i, j) == 1 � � ��� � � � ��� � � ��������������� ; end if end for � � ��� � � � ��� � t � /�� ������������� �; � � ��� � � � ��� � t � /�� ������������� �; fitness_value=1/L(� � , � � ); end for The process of computing L�t � , t � � will vary with the size of the t � , t � . Therefore, we need to discuss the situation separately as Figure 3. Thus, individuals with greater fitness are more likely to be selected into the next generation of individuals. At the same time, elite mechanism is utilized that individuals with the highest fitness per generation are automatically reserved for the next generation.
Crossover function selects two individuals in the new species group selected by selection function as parent individuals, and takes any point from the two extracted individuals as intersections. If their crossover probability (cross_rate) allows the two parent individuals to cross, they start at that point until all parts after that point are cross interchanged as 0. Similar to B, A should first give a point that needs to mutate, and then substitute the other alleles for the gene at that point, which is usually a simple reverse operation as 0.
The 6 nodes are selected randomly in the threedimensional space as 0 to verify the universality of the algorithm. The algorithm parameters are set as follows,pop_size = 30, chromo_size = 10, generation_size = 300, cross_rate = 0.8, mutate_rate = 0.02. The un-optimized path indicates the distance at which all joint parameters move at the same time from the t=0 moment. Ten node sequences are used for optimization among five nodes as 0. The experiments results show that the movement distance is optimized greatly by the algorithm, which is reduced by 37% compared to un-optimized condition. When the precision is set as 1/1000, the code runs from one and a half minutes to two minutes. Figure4. The comparison of un-optimized path and optimized path of Node 1-3is shown in Figure 4. To overcome this drawback, the Taguchi method of design of experiment (DOE) is used in this study, which uses orthogonal arrays to decrease the number of experiments [10]. According to the number of parameters and the number of factor levels, the orthogonal array L �� �� � � in 0 is selected for executing the experiments.  In different factor lever, the running time of the algorithm is between one and a half to three minutes, and the convergence of the L is stable and has no obvious fluctuation. As shown in 0, the algorithm converges to the

Conclusion
In this paper, an optimization scheme for joint motion time series is proposed on the basis of the Stanford Manipulator's mathematical model. Under the specified conditions, the minimum path of the end effector from the prescribed start point to the end point is obtained by applying the GA optimization method and performing three polynomial interpolation of the joint variables.
Genetic algorithm has good repeatability and stable solution, and is not easy to generate local optimal solution. However, the optimization effect of the algorithm is related to the predetermined position. The reason is that all joints can only reach the finishing line at the same time. In the future work, more complex joint time series constraints can be taken into consideration to provide a good optimization effect for the first and last positions.
Although the problem of obstacle avoidance is not considered in this paper, the algorithm can set up a barrier space, and then sift out all the individuals entering the barrier space in the process of population selecting. This provides some feasible direction for further research.