A Dual-Thread Method for Time-Optimal Trajectory Planning in Joint Space Based on Improved NGA

To solve the problem that the time-consuming optimization process of Genetic Algorithm (GA) can erode the expected timesaving brought by the algorithm, time-optimal trajectory planning based on cubic spline was used, after the modification to classical fitness sharing function of NGA, a dual-threaded method utilizing elite strategy characteristic was designed which was based on Niche Genetic Algorithm (NGA) with the fitness sharing technique. +e simulation results show that the proposed method can mitigate the contradiction of the long term the optimization algorithm takes but a short running time the trajectory gets, demonstrating the effectiveness of the proposed method. Besides, the improved fitness sharing technique has reduced the subjective process of determining relevant parameters and the optimized trajectory results met performance constraints of the robot joints.


Introduction
Genetic Algorithm (GA), capable of avoiding local minimum, independent of gradient information, is extremely suitable for large-scale complex optimization problem that traditional methods found hard to model and solve. In the actual industrial application field such as package-palletizing and workpiece-welding, a large amount of GA variant has been developed to help obtain the industrial manipulators' performance-optimal trajectory in some kinds of specification.
Dong et al. [1] and Chen [2] applied GA to path optimization for palletizing manipulators, improving manipulators' efficiency. In this scenario, the palletizing work of manipulators can be classified as a pick-and-place job. Liu et al. [3] used vision-based industrial robot improvement GA, planning the trajectory of pig abdominal cutting. Hou et al. [4] used a GA based multiobjective optimization algorithm to study the workpiece welding problem of a tworobot cooperative system. An improved multi-island migration GA was utilized by Du et al. [5] to plan the falling motion of a crossbeam in a hydraulic press system, so as to obtain the trajectory with strong pressure shock suppression ability. Ren et al. [6] used a GA to solve the motion planning problem of asynchronous parallel disassembly (aPDP) in regenerative manufacturing, which is a promising technology compared with serial disassembly.
Most of the current research studies are devoted to accelerating the local convergence of GA and extending its ability to search on the global solution space. Two main categories can achieve the target, first of which would be combining GA with another optimization algorithm. Since GA has the global optimization ability, quite a few methods utilized an algorithm with prevailing in local search to combine. Zhang et al. [7] combined whale optimization algorithm (WOA) with GA by adding WOA operations to solve the dynamic modeling problem of manipulators. A similar method was also proposed by Zhou et al. [8], in which grey wolf optimization (GWO) was utilized, and grey wolf hunting behaviors were introduced before GA's operations. e proposed hybrid GA with GWO can provide input weights and biases of an extreme learning machine. e quasi-Newton solver is a classical method with fast local convergence, and it was combined with GA by Khawli [9] to complete the calibration job of a manipulator with the help of a laser tracker. In [10], independent evolution and atavistic migration mechanism with duo populations have been introduced to GA, in which the Chaos Search Algorithm (CSA) was utilized to provide newly born individuals to populations periodically. ere are also hybrid GA algorithms regarding the premature defects of GA as the advantage of fast convergence. In [11], the CSA was used to traverse the entire solution space with the purpose of narrowing the possible scope where the optimal solution would appear, while a GA with premature defect completed the local search. e second category would be hybrid algorithms that get heuristic from bioecology or other academic disciplines to improve GA. e muti-island migration model of IMGA in [5] is derived from the theory of natural migration, in which populations are isolated by imaginary islands and reproduce independently. After a few generations, individuals migrate between islands, guaranteeing genetic diversity. Liu et al. [12] adopted a concept of elitist groups to overcome the premature defect and enhance the global searching capacity of GA by replacing the worse group with the most elitist group. e quantum genetic algorithm (QGA) was used by Chen and Zhou [13] to perform trajectory planning for a space manipulator with a floating base. It borrows the concept from quantum theory and encodes GA's chromosome with quantum states, while population propagation is accomplished by a quantum rotate gate, enriching GA's genetic diversity.
e purpose of optimal time optimization is to shorten the trajectory runtime, improving efficiency and productivity. However, the heavy calculation burden and timeconsuming optimization process of GA has a negative effect on the efficiency, which will erode the results of the algorithm. Research studies on how to improve the practicability of GA in practical implementation are seldom mentioned. When using GA to plan an optimal trajectory for manipulators, the conventional method always arranges the optimization algorithm and manipulators' execution algorithm into a sequential structure. More specifically speaking, the optimization results will not be returned unless the GA termination conditions were satisfied, after which manipulators continue executing the optimal trajectory. In reality, however, the trajectory may be accomplished in less than a few seconds, but optimizing it often takes multiple times as long as its original runtime. If the trajectory only needs to be executed once, it may not be worth optimizing it, especially in those typical manufacture scenarios. Even for repetitive trajectories, it is necessary to consider whether the improvement brought by the investment in optimization time is cost-effective. In the case of inconsiderable improvement, it is also futile to invest time resources to optimize the timeoptimal trajectory. erefore, the problem of whether the termination condition of the algorithm is reasonable and the granularity of the traversal domain is appropriate under the conventional scheme is particularly prominent. e negative result of the coarse design of the termination conditions is that the optimization process time will negate or even devour the ideal advantage of trajectory optimization.
In this paper, the Niche Genetic Algorithm (NGA) with elite strategy and modified fitness sharing function of NGA was adopted to construct a dual-thread algorithm concerning parallel processing of the optimal trajectory optimization task and the real-time execution task of robot joint space. It is proposed to solve the problem that the coarse design of the termination conditions and the granularity of the traversal domain can erode the expected time-saving when GA is used routinely in the situation that the trajectory running time relative to the optimization process is short-lived.

Time-Optimal Trajectory Planning Based on the Cubic Spline
Generally speaking, to reduce the impulse shock to robot joints, ensuring the end effector of robots moving smoothly, a higher-order smooth function should be chosen as the interpolation function, which is devoted to calculate the interpolating trajectory between given points. Attention should be paid while designing the optimal purpose. e optimal trajectory runtime was chosen in our research since efficiency is always the first index to industrial robots. Last but not the least, proper simulation results can help to choose robot joint actuators economically. For these reasons, the joint position commands sent to the servo system carrying time information, which were always obtained through inverse kinematics calculation and interpolation, should try to exploit the potential of the driven system in the field of velocity, acceleration, and jerk limitation but also in the range of joint position bounds. In such considerations, the trajectory is optimized to the maximum extent concerning the upper limit performance of the manipulator body. e cubic spline was chosen as the core interpolation function since it has the property of C 2 conductivity, ensuring continuous acceleration, and it can be wrapped into a matrix form exactly suitable for optimization using a genetic algorithm. If higher-order continuity and smoother trajectory were pursuing, the B-spline function could be another competitive candidate.

Matrix Form of Joint Space Trajectory Using the Cubic Spline.
e mathematic solution of joint space trajectory planning using the cubic spline can be derived from the 3rdorder polynomial trajectory planning method [14].
Consider the three continuity conditions that the intermediate point should have: q(t), v(t), and a(t) are continuous along the trajectory. Use the continuity of velocity to write linear equations in a matrix form, as shown in equation (1). By substituting joint position vector q V → representing all path points whose length is n, along with time interval vector h → between path points, initial and final velocity v s , v e , and also initial and final acceleration aV s and aV e , consequently, the acceleration vector a V → at the spline junction meeting the boundary conditions can be calculated. e coefficient matrix A and the conditions placed at the right hand side with vector form b of the formula in equation (1) were, respectively, represented as equations (2) and (3).
Based on the fact that the location continuity constraint is not clearly defined at the beginning and the end of the trajectory, 2 virtual points should be introduced: one at the beginning and the other at the end. After adding these two virtual points, the number of elements of the position vector q V → comes up to n + 2.
e actual positions of these two virtual points are not explicitly specified but are rather determined by other current information based on continuity conditions, as shown in the formula in equation (4). e cubic spline curve ensures the continuous requirement of joint space motion in the field of position, velocity, and acceleration, which can effectively reduce joint stiffness and flexibility impact. . . . 0 a n− 3,n− 4 a n− 3,n− 3 a n− 3,n− 2 0 . . . . . . 0 a n− 2,n− 3 a n− 2,n− 2 where, Once all the accelerations are determined using equation (1), they can be substituted into the cubic spline interpolation formula (equation (5)). According to the predetermined control period, the input time variable t can be used to calculate the joint position and velocity that need to be periodically output to the servo system for execution.

Expression of Time-Optimal Trajectory Goal and
Constrains. From the point of view of productivity, the joint space constraint condition of the robot was set, which was guided by the motor and reducer to give a full play to their potential. e optimal trajectory running time is considered as the optimization objective, and the natural language expression of its constraint conditions is the corresponding velocity, acceleration, and jerk of each joint at every trajectory point do not exceed the upper limit of joint performance. e optimization target and constrains were shown in equation (6), where i is the index of time intervals, while j is the index of joints; moreover, jC j aC j , and vC j , respectively, stand for the upper limit of the jth joint in the field of jerk acceleration and velocity. According to [15], the maximum of velocity acceleration and jerk along the trajectory can be calculated, which is also shown as follows:

Modification of NGA
Niche is a concept borrowed from the biology field, meaning those habitats and behaviors acclimatized to species. Genetic algorithm with niche behavior can be regarded as Niche Genetic Algorithm (NGA). For now, NGA has the ability to effectively increase differences among individuals, enriching the diversity of population, which has ensured genes from elites will not govern the whole population. Many methods can be exerted to swift the standard genetic algorithm into a niche genetic algorithm, the first of which is the Fitness Sharing technique [16]. e fitness sharing technique is a common solution solving Pareto Front in a multiobjective optimization problem. e second common technique is Direct Crowing [17]. It would use some metric to divide the whole population into several small populations. Each small population individually produces its offspring, and resources competition would happen between offsprings and parents. Only the winner can be reserved. Different from these two gentle techniques mentioned before, Individuals Clearing [18] is a much more radical technique. It only reserves the best individual in the same niche population, in some perspective, meaning radical evolving.

Modification to Fitness Sharing Function.
In this paper, the NGA algorithm with an elite strategy was implemented, in which niche behavior was developed from the classical fitness sharing technology. Fitness function was used to estimate niche crowding quantitatively. e classical fitness function is shown in equation (7), where d i,j is the distance (chromosome difference) between the ith individual and the jth individual; moreover σ s is the radius of the niche, which is typically related to the maximum distance between individuals in the population, and the classical value of α is 1, which is usually used to control the steepness of the fitness function.
e resource allocation manner of the classical fitness function is linear for different individuals in the same niche. is manner would lead to an underestimation of the crowding level between individuals with less chromosome difference, but an overestimation of those individuals with larger chromosome difference.
Introducing niche behavior to GA can significantly reduce the number of similar individuals in the population, enriching the diversity of the gene pool in the population. For the two individuals with a small d value, it is expected to have a severe crowding degree estimation, to impose a higher penalty degree on the suboptimal individuals and reduce the probability of homogeneous genes being inherited to the next generation. Similarly, the two individuals with an immense d value are expected to have a moderately small crowding estimation. e general practice to achieve this purpose is adjusting the α value [19]. A sharper fitness function can be obtained by increasing α. However, there remained some extra considerations in determining α, and the optimal value is not easy to determine.
As shown in Figure 1(a), the classical fitness function and the standard normal distribution curve have a very high similarity of contour. Based on the fact that normal distribution has been widely used in simulating natural systems, this paper proposes to replace the classical fitness function with modified normal distribution function to reduce the subjective process of quantitative estimation of crowding degree. sh m (d) represents the fitness function after modification. To be used as the fitness function expression, two basic requirements of the original classical fitness function should be satisfied: first, the crowding degree of two same chromosomes equals to 1; second, when the difference of two chromosomes exceeds the niche radius, the crowding degree is estimated to be near and asymptotically zero.
ese two requirements can be expressed as sh m (0) � 1, In order to satisfy the first requirement, a deviation to the whole and a stretching on the amplitude can, respectively, apply to the Gaussian distribution; at the same time, using the "three-sigma principle" of the Gaussian distribution, let the three times of the standard deviation equal to the radius of the niche to meet the second requirement. us, equation (9) can be listed and substituted into equation (8) to calculate the coefficient K and the standard deviation sigma, as shown in equation (10). e fitness function based on the Gaussian distribution function after modification is shown in equation (11), and its comparison with the classical fitness function is shown in Figure 1

Flow Description of NGA with Elite
Strategy. e complete flow chart of the NGA algorithm is shown in Figure 2, and the algorithm steps are described as follows: Step 1. (Population Initialization). A massive matrix representing the whole population should be initialized, while its number of rows is 1 more than the actual number of path points, and its number of columns equals the size of the population; each column is declared as an individual.

Journal of Robotics
Step 2. (Raw Fitness Calculation and Constrains Judging). Four bounding constrain judgments, including position bounding, velocity bounding, acceleration bounding, and jerk bounding, would be applied to every individual. Individuals who violate any of these constraints would be abandoned. If all constraints are satisfied, raw fitness scores would be calculated, which is exactly equal to trajectory traveling time.
Step 3 (Fitness Sharing) e fitness sharing technique can convert raw fitness scores into shared fitness. e function which was devoted to this conversion is termed fitness function. e classical fitness function is shown as equation (7) and the modified one is shown as equation (11). Also, the sum of fitness function values of individual i relative to j (including itself ) is termed Niche Count, which would be used in the conversion of raw fitness. e niche count is a proper index for showing how heavily the competition has happened among individuals in the same niche. e larger of the niche count, the heavier of the competition.
Step 4 (Selection Operation). e unified roulette selection method would be used to choose the parent set from the population, who would have chances to produce offspring.
Step 5 (Population Propagation). e elite operation, crossover operation including mutate operation, would be simultaneously applied on the parent set, which is obtained from the previous step, producing three types of offspring constituting the next generation.
Step 6 (Termination Judging). NGA stopping criteria is judged in this step. If criteria are met, the optimization process finishes and otherwise jumps back to Step 2 and continues.

Dual-read Method Using the Characteristic of Elite
Strategy. In the elite strategy, the elite individuals in each generation would directly enter the subsequence generation, and the most elite individual is the latest optimization results of the manipulator trajectory. ese early optimization results may be eliminated from individual elite groups in the later stage with the birth of better results, but in fact, they are all feasible results, which is a kind of underutilization of resources in some sense. If the application method of GA is designed as a dual-thread method that both the manipulator worker thread and the GA optimization thread work simultaneously, the GA optimization thread can provide the optimal individual results of each generation for the robot worker thread to use at the earlier stage, and as a result, resources can be fully utilized. e shared memory model, shown in Figure 3(a), plays a communication medium role, as well as Figure 3(b) shows the dual-thread framework in which the robot trajectory execution thread and the computational thread for trajectory optimization work simultaneously. e framework presented in Figure 3    working thread, the optimal-trajectory planning thread, and the shared memory area. Among them, the manipulator working thread (MWT) searches the executable trajectory information in the shared memory area and performs them as the real-time movement task; the optimal-trajectory planning thread (OPT) performs NGA with the elite strategy to optimize the joint space trajectory of the manipulator; the shared memory area (SMA) accomplishes data exchange missions between threads. However, the realization of obstacle avoidance and path geometry requirements of Cartesian space, as well as the transformation to joint space trajectory, are provided by the path planning algorithm of a higher level. Figure 3(a) shows the three partitions of SMA in order:(1) the memory block for semaphore (S Block), which is used to set the data access permission between processes, (2) the memory block for optimization results restoring (R Block), which were calculated by OPT and would be transferred to MWT, and (3) the memory block for trajectory constrains restoring (C Block), which is devoted to storing all constraints about trajectory, including geometry constrains, kinematic constraints, and dynamic constraints; last but not the least, data inside C Block are shared from MWT to OPT. e dual-thread framework makes full use of elite individuals in each generation. e optimal elite individual in each generation of the population is shared to the MWT periodically through the SMA. e MWT can be started to execute the optimized trajectory before the final calculation of the OPT is completed. Moreover, the robot controller can obtain more and more optimized update trajectory from the OPT when dealing with a repetitive path. is framework can effectively solve the problems caused by improper design of algorithm termination conditions and traversal granularity when adopting a genetic algorithm for trajectory timeoptimal planning and further improve the practicability of genetic algorithm in industrial application.

Description of Simulation Environment.
e robot object used in the simulation experiment is a Puma-like manipulator STR6-05, and its d-h parameters are shown in Table 1.
e corresponding joint constraints are listed in Table 2. e path object going to be optimized in the simulation experiment is the same path described by multiple intermediate points, as shown in Table 3.

Normal Framework vs. Dual-read Framework.
In order to prove the effectiveness of the proposed dual-thread framework, a simulation comparison experiment is carried out. By using the conventional single-thread method and the dual-thread method proposed in this paper, the execution efficiency of the simulation manipulator on a same trajectory (execution counts in the same limited time) was the comparing index.  Especially for the dual-thread method, as shown in Figure 4, the MATLAB memory-mapping file can help realize the simulation of the dual-thread method. e core is the memory mapping function "memmapfile" from MAT-LAB, which creates a mapping from a file on the hard disk to a piece of continuous memory. After establishing the mapping, what is stored in the memory is the filehandle to the corresponding location of the disk file and the length of the preset data element. After the process obtains the filehandle and relevant data length information in memory, it can access and modify the data in the specified position in the file in real-time, so as to realize the real-time modification of the same disk file by different program processes, that is, using the memory-mapping file to simulate the SMA. e MWT and the OPT individually occupy an independent MATLAB process to run but establish a unified memory structure to the same memory-mapping file. e MWT simulation process has run a manipulator simulation GUI, which was developed based on the MATLAB RTB (Robotics ToolBox) [20], providing kinematic simulation verification with the manipulator linkage model, as shown in Figure 5. On the other hand, the OPT simulation process has run an NGA based on the GA algorithm in MATLAB Global Optimization Toolbox. e specific processing flow between the two MATLAB processes obeys the suggested dual-thread framework.
As for the single-thread framework, the MWT and the OPT run in the same MATLAB process and have the sequential execution relationship: the simulation GUI needs to wait for the completion of NGA calculation, and the final result must be obtained before the execution of trajectory.
Several comparative groups of the single-thread framework and one dual-thread framework group with different termination conditions were designed. eir optimization algorithm termination conditions and NGA parameters setting are shown in Table 4. Since the maximum distance (MD) between individuals changes as the population evolves, if the niche radius was set relevant to MD, additional uncertainty was introduced to the population evolution. In order to reduce uncertainty, a fixed niche radius was used in the simulation experiment. After trial and error, 0.1 is a nonoptimal but reasonable setting value for the niche radius. Nine rounds of simulation experiments were carried out for each group, with the maximum running time set for each round ranging from 5 minutes to 45 minutes. Figure 6(a) shows the corresponding simulation experiment results of 5 groups. An intuitive conclusion could be drawn: the trajectory execution counts of the dual-thread framework are higher than that of all single-thread framework groups. At the situation of the preset maximum running time of t set = 5 min, the trajectory optimization of all single-thread groups has not given the result; there is no   Journal of Robotics executable trajectory, so the current execution counts of repeated trajectories are all zeros since the MWT is sequential running after the OPT. e dual-thread framework effectively utilizes the optimal elite individuals in the early stage of the population, so that the MWT can obtain an acceptable executable solution in advance and start the task earlier than the conventional single-thread framework. Figure 6(b) shows the execution counts varying with time for each group in a single round where the maximum running time t set = 35 min. From Figure 6(   point between the curve and the X-axis mean the earlier the trajectory task startup time. e trajectory execution efficiency of each single-thread framework group was 3.42 counts/min, 2.45 counts/min, 3.30 counts/min, 3.36 counts/ min, respectively, corresponding to threshold setting to 10 − 3 , 10 − 8 , 10 − 12 , 10 − 20 , and the execution efficiency of dual-thread framework group increased from 2.72 counts/min at the first stage to 3.06 counts/min at the last stage, with an efficiency increase of 12.5%. Since the second joint has a broader range of motion and massively influences the position of the end-effector, it has been picked as the representative joint. Figures 7(a) and 7(b), respectively, show the comparison results of the final trajectories of the dual-thread and single-thread framework with the second joint of str6-05 at t set = 35 min. e j/a/v/p symbols on the vertical axis, respectively, represent the jerk, acceleration, velocity, and position of the joint. e trajectory curve has conformed to the cubic spline curve. According to the final time result of its horizontal axis shown and Figure 7(a), it can be seen that although the trajectory optimization result of the single-thread framework is slightly better than that of the dual-thread framework, the trajectory execution counts of the dual-thread framework are still more. e randomness of the GA result causes the unfortunate dual-thread result in this round. However, as long as the running time continues to be extended, the dual-thread framework still has the chance to produce and update better results.

Comparison Among Different Fitness-Sharing Schemes.
With the purpose of comparing the effects of using modified fitness function, classical fitness function, and no niche technology, the aforementioned dual-thread framework was adopted, and the termination condition of the algorithm was reaching the upper limit of the preset maximum running time t set � 60 min, or the relative changes of the best elite individual are less than the threshold 10 − 20 . In groups with niche behavior, the niche radius σ s was 0.1. In the later stage of the simulation experiment, the population matrix data would be recorded. e scatter graph depicts the distribution of raw fitness scores for the whole population, in which the distance calculation of each individual was relative to the best elite individual in the generation. A histogram was further introduced to the distribution. Two graphs have been drawn on the same figure. en, zooming in on the part within the niche radius σ s , observation would be performed for analyzing the effect. Figure 8 shows the results, and in Figure 8(a), sh m (d) is applied instead of sh(d) to fitness sharing, and in Figure 8(b), sh(d) fitness sharing is applied; moreover, in Figure 8(c), fitness sharing is not used. According to    , by observing the histogram of scatter points within the radius s of niche after zooming in, it can be seen that the phenomenon of population clustering near the optimal individual appears obviously without niche technology, and the closer to the optimal individual, the higher the clustering degree. is phenomenon would eventually lead to "premature" genetic algorithm and deterioration of the dual-thread framework proposed in this paper. By comparing the X-axis coordinate range of the original Figures 8(a), 8(b), and 8(c), it can be found that the range span of the population is larger after the use of niche technology; furthermore, the proposed modified fitness sharing has the most extensive range span of the population, signing that the NGA with modified fitness sharing can effectively increase the genetic diversity of the population. By further comparison on the population distribution of the two different fitness sharing behavior, it can be seen that modified fitness sharing can effectively enhance the punishment applying on the individuals who are closer to the optimal individual since more individuals appeared near the border of the radius. As a result, the species gene pool would not massively be occupied by the optimal individual genes. For individuals whose location in the distribution is less than zero, it is either an individual that violates the constraint and would be eliminated in the next generation or a new feasible result that is better than the current optimal one.

Conclusion
(1) e modification of the classical fitness function based on the Gaussian distribution was proposed to reduce the subjective process of adjusting the effect of fitness function while rationalizing the estimation of population individual crowding degree. It also helps the NGA algorithm get rid of the "premature" defect. (2) e dual-thread framework utilizing the characteristics of the elite strategy of NGA is proposed to make exhaust use of the early-stage optimal elite individuals in each generation of population, to solve the problem caused by the course decision of the termination conditions of genetic algorithm and the difficulty in determining the granularity of traversal domain.
Data Availability e data, simulation tool without third-party libraries, and developed algorithm script of this study are available from the corresponding author upon request.