Correction: Path planning of industrial robots based on the Adaptive field cooperative sampling algorithm

Zhuang Yong BoLuo Sha^*Li Qing Dang^*Chu Dian MingBai Wen JuanLiu Xin TaoLv Wei

• Qingdao University of Science and Technology, Qingdao, China

Path planning is a decisive factor to determine their safe operation. It refers to the autonomous planning of industrial robots in their configuration space to find a continuous non-collision smooth path between the initial pose and the target pose in order to reach the preset target pose when moving in the surrounding static and dynamic obstacle environment. Meanwhile, it must meet various constraints such as environmental, time and dynamic constraints of industrial robots [1]. But the path planning algorithm of mobile robots is not completely applicable to the path planning of industrial robots. At present, there are three kinds of path planning algorithms of industrial robots: traditional obstacle avoidance planning method [2]- [3],intelligent obstacle avoidance planning algorithm [4]- [5],sampling-based obstacle avoidance planning algorithm [6]- [7].Among them, the sample-based RRT algorithm [6] is the most applicable one in path planning algorithms of industrial robots, who has the characteristics of probability completeness and high dimensional space applicability [8]. And it was also proved that the sample-based RRT algorithm owned the better efficiency and more smoother path than the intelligent obstacle avoidance planning algorithm for the industrial robots [33].However, the traditional RRT algorithm has some problems such as redundant sampling, low efficiency and non-optimal path, which limit its generalization ability in complex environment [9]. For these problems, research and improvement of RRT algorithm is a hot topic pursued by many scholars.Among the variants of RRT algorithm, neither RRT algorithm, RRT* algorithm [7] nor Informed RRT* algorithm [10]fundamentally solve the randomness, low efficiency and optimality.Especially, in the sampling stage, redundant sampling brings a large number of branches, which not only leads to low efficiency, but also occupies a large amount of memory space. Therefore, based on the above algorithms, scholars have make some research on RRT algorithm from the aspects of sampling and expansion strategy. In terms of sampling strategies, the paper [11]- [17]adopted target paranoia to achieve "de-randomness". But this strategy only reduced a small amount of the redundant sampling . Therefore, to further reducing the sampling space, Liu Ben [18], Wenzheng Chi et al. [19] and Sivasankar Ganesan [20] set the sampling interval of the traditional RRT algorithm in different ways to make the search more efficient. On the aspect of the extension strategy, the paper [13]- [17] fused the RRT algorithm with APF algorithm. Specifically, it used the attractive action to guide the production of new nodes and improve the search speed of the algorithm. In this process, although the target attraction is introduced, the repulsion of obstacles is not considered, so the role of the APF algorithm cannot be fully simulated.Wang Dong et al. [12], Kabutan. R et al. [21]get new nodes by the guiding effect of target gravity and obstacle repulsion. It improved not only the goal orientation but also the obstacle avoidance ability. But with the disadvantages of unreachable target and local minima, the APF algorithm made the RRT algorithm unable to find new sampling points in some cases [22]. Therefore, a path planning algorithm with strong stability, high efficiency and strong generalization ability is urgently needed for the characteristics of high latitude and complex collision detection process of industrial robots.To get more improvement on the adaptability and stability of the path planning algorithm, an APCS algorithm is proposed, which combines the efficient search and optimization ability of the improved APF algorithm with the completeness of the RRT algorithm. And it fundamentally solves the problem of environment adaptability and redundant sampling. Contributions of this paper are summarized as follows:•An environment complexity function is proposed to make path planning algorithm adapt to the environment； •An optimal sampling strategy is proposed, which not only makes full use of environmental information, but also considers the optimality of sampling points； •A field cooperative expansion strategy is proposed,in which the improved APF algorithm is proposed and guides the generation of new nodes;•A new algorithm called AFCS is proposed ,which not only enhances the environmental adaptability of industrial robots,but also improves sampling quality and scaling efficiency of the path planning algorithm. The whole algorithm provides ideas for improving the intelligence of the path planning algorithm.The rest of this article is arranged as follows:The principle of path planning of robots is analyzed,and the principle of the traditional path planning algorithm is introduced in section 2.Then the AFCS algorithms is introduced in section 3. Section 4, summary and analysis. For industrial robots, it is a prerequisite for path planning to satisfy constraints in terms of kinematics and dynamics. In this article, the relevant theories of industrial robot path planning are further studied based on the full study of the kinematics of industrial robots. Path planning of industrial robot needs mutual transformation in joint space and Cartesian space. So it involves the solution of inverse kinematics. And the accuracy of the inverse kinematics solution also certainly affects the accuracy of path planning of industrial robots. In this paper, ROKAE 6-DOF industrial robot is taken as the research object, and the coordinate system is established according to the DH model [23]as shown in Fig. 1. i i i i i i i i i i i i i i i i i i i a a T d                               i represents the angle between the X axes of adjacent coordinate systems, The core of path planning is planning, and its goal is to obtain a path that satisfies the conditions. The path is a continuous curve in the configuration space of robots. Specifically, path planning may be to plan a non-collision and shortest path for mobile robots in two-dimensional space. It also can plan a safe, non-collision or relatively optimized path for industrial robots . JiangXinsong, the father of Chinese robots, defines path planning as follow:The goal of path planning is to get a non-collision path in the environment with obstacles according to certain evaluation criteria [24]. Deservedly, the different distribution of obstacles in the environment directly affects the planned path, and the target location determination is provided by the higher-level task decomposition module [24]. In traditional path planning, environment modeling is the first step of path planning in order to better satisfy configuration space constraints. However, it is more and more difficult to model the environment in the more complex scenario. The complexity of environment modeling is an important problem that restricts the path planning of robots. This article puts forward the environmental complexity function to counter the difficulty of environment modeling. This strategy realizes the prediction of the environment where the path planning is located, and lays a theoretical foundation for path planning algorithm to take full advantage of the environmental information. This paper designs two kinds of environment maps (as shown in Fig. 3), in which the superiority of path planning algorithm can be better reflected. As shown in Fig. 2, the number of obstacles in two maps is the same, but the complexity of the environment is different because of the different distribution of obstacles. The pixel size of the two maps is1500*1500. In this paper, the starting point position coordinate of path planning is (0,0,0), and the target point coordinate is set to (1000,1000,1000). The sampling-based RRT algorithm and APF algorithm are the most widely used in path planning of industrial robots. By fully studying the traditional APF algorithm and RRT algorithm, this article summarizes the advantages and disadvantages of the two methods. At the same time, this article makes same improvement of the traditional algorithms in view of their shortcomings, and a new AFCS algorithm with strong environment adaptability is proposed. There is a path search algorithm called the RRT algorithm, which expands by random sampling. It establishes the search tree from the starting point. Subsequently, it undergoes random sampling, expands new nodes, avoids obstacles, and finally finds an optimal path. Fig3 shows the implementation process of the traditional RRT algorithm, and its pseudo-code is shown in Fig. 4:Fig. 3 The implementation of the RRT algorithm Fig. 4 The pseudo-code of the traditional RRT algorithmThe core of traditional RRT algorithm can be divided into four stages: sampling, expansion, collision detection and path query. There are three important factors that lead to the low efficiency and poor applicability of RRT algorithm, which are the redundant sampling in the sampling phase, the way and efficiency of generating new nodes in the expansion phase, and the amount of calculation in collision detection [25]. Especially in the sampling stage, the redundant sampling not only has strong randomness, but also does not consider the optimality of sampling. Moreover, in the measurement connection stage, the generation of new nodes is neither guided nor considering the influence of obstacles, so that the calculation of collision detection of the RRT algorithm is very large. Generally, the randomness and efficiency of the RRT algorithm are important factors affecting its development. Therefore, this paper proposes an efficient RRT algorithm to solve the strong randomness and low efficiency.( The APF algorithm is a simple and effective local path planning algorithm. It constructed by Dr. Oussama Khatib who introduced the concept of "field" in traditional mechanics. The potential field includes the attractive and repulsive potential field, which are formed by the target and the obstacle respectively. The agent moves along the resultant force of the attractive and repulsive forces [26]. The attractive and repulsive potential field functions are modeled as follows: The model of the attractive and repulsive forces are shown as follow: ) (3 2 1 ) ( goal t att P P k P U   ) 4 ( ) 1 1 ( 2 0 2 2 2           ) (5 ) ( ) ( goal t aat att P P k P U P F      ) 6 ( ) 1 1 ( 0 ) ( 2 2 2                The resultant force of attraction and repulsion is shown as follow:) 7 ( ) ( ) ( ) ( P F P F P F rep att total The agent realizes obstacle avoidance path planning under the guidance of the resultant force, and its schematic diagram is shown in Fig. 5. On the basis of the principle of the APF algorithm, it can be known that the magnitude of the attraction and repulsion force of the agent are related to the corresponding distance. So its main disadvantages are the inaccessible target and local minimum [27]. Otherwise, the essence of the APF algorithm is a control method, and its path is generated by the control quantity in real time. Therefore, its real-time performance is more strong. For the shortcomings of the APF algorithm, this paper proposes an improved APF algorithm, and introduces it into the traditional RRT algorithm to improve its real-time performance . On the principle of path planning algorithm, this paper innovatively put forward an adaptive field cooperative sampling algorithm. Firstly, the environment complexity function is proposed for the difficulty of environment modeling. Then,an improved APF algorithm is proposed to solve the inaccessible target and local minimum. Finally, based on the framework of the traditional RRT algorithm, an optimal sampling strategy is proposed, and the AFCS algorithm is obtained by introducing the environment complexity function and the improved APF algorithm into the RRT algorithm.(1) Environment complexity function Aiming at the complex of environment modeling, this article designs the environment complexity function S based on the relative position of obstacles in the environment. Then it is introduced into the traditional RRT algorithm. This operation enables the algorithm to intelligently adjust the iteration time instead of manually adjusting the iteration parameters according to the complexity of the environment. And it also improves its environmental adaptability . On the basis of the volume ratio of obstacles, this article introduces the distance between obstacles to further distinguish the complexity, so as to make the path planning algorithm suitable for environments with different complexity. The environment complexity function is as follows:) 8 ( S 3 10 1 1 1 VL e V n obs  Where, S represents the environment complexity.1 n respectively indicates the number of static in the environment [28].1 obs V represents the volume of static obstacles. V represents the total volume .Only one class of static obstacle cases is studied here, so 1 1   . According to the proposed environment complexity function, the environment complexity of the two maps in Fig. 3 can be respectively calculated as: 2301, 1809. Although the number of obstacles in the two maps is the same, the relative position of obstacles is different. In the green sphere environment, the minimum distance between obstacles is 697mm. In the red sphere environment, the minimum distance between obstacles is 883mm. The green sphere environment is relatively complex. Therefore, the design of the environment complexity function is reasonable.(2) Optimal sampling strategy For the RRT algorithm and its variants, the ideal sampling procedure is one that reduces redundant sampling while keeping the sampling point on or near the optimal path as much as possible. Based on the above ideas, this article proposes an optimal sampling strategy to solve redundant sampling and poor quality of sampling points in the sampling stage. The idea of this strategy is to randomly generate multiple sampling points at the same time, and then determine the sampling quality function based on the density of obstacles around the sampling points and the smoothness of the path. In this way, random sampling points with optimal quality can be obtained. The schematic diagram of optimal sampling process is shown in Fig. 6: Fig. 6 The schematic diagram of optimal sampling process( The blue path represents the optimal sampling process, and the black path represents the path generated by the random sampling process ) For the problem of the inaccessible target and local minimum, this article respectively introduces the distance between the target point and the robot into the attractive potential field and the repulsive potential field function of the traditional APF algorithm. Based on papers [29]- [32], the attractive and repulsive potential field models proposed in this paper are shown as follow: ) (12 2 2 ) ( 1 2 1 1 1              ) (14 ) ( 1 1 1 1                Based on the above innovative strategy, an adaptive field co-sampling algorithm is proposed in this paper. It not only avoids complex environment modeling, but also makes full use of environment information. Moreover it solves the redundant sampling and low efficiency of traditional RRT algorithm by the optimal sampling strategy and the introduction of improved APF algorithm. The flow chart of the AFCS algorithm is shown in Fig. 8. The pseudo-code of the AFCS algorithm is shown in Fig. 9: Fig. 9 The pseudo-code of the AFCS algorithm Simulation experiment is an important way to show the superiority of the algorithm. The simulation experiment in this paper is carried out in two aspects: algorithm simulation and industrial robot simulation.In terms of algorithm simulation, two environments with different complexity are used to carry out simulation experiments to verify the environmental adaptability of the algorithm. At the same time, this paper verifies the effectiveness and practicability of the AFCS algorithm by analyzing the experimental results with the traditional RRT algorithm, RRT* algorithm and tRRT algorithm from three aspects: planning time, path cost and the number of path points.In the aspect of industrial robot simulation, this paper mainly integrates the AFCS algorithm with industrial robot simulation to verify its effectiveness and practicability .In two maps with different complexity, this article conducts multiple simulation experiments with the same number of iterations and step size respectively. The multiple operation results of the traditional RRT algorithm and RRT* algorithm are shown in Fig. 10 and Fig. 11. Therefore, this article focuses on the comparison and analysis of simulation results between the AFCS algorithm and the most representative tRRT algorithm. Environmental adaptability is an embodiment of the intelligence of path planning algorithm.It is also an important direction to improve the intelligence of industrial robots. Five consecutive simulation experiments are carried out in two environments with different complexity(as shown in And the branches of the path obtained by the AFCS algorithm are less. But when the environment of the tRRT algorithm changes(from red sphere environment to green sphere environment),the branches of its path becomes more and more. Therefore, the proposed AFCS algorithm has better adaptability to the environment and higher stability. Time is an important reflection of efficiency. In this paper, the planning time of the algorithm in the same environment is taken as one of the criteria to measure the efficiency of the algorithm.The AFCS algorithm and tRRT algorithm are run 10 times in two environments with different complexity to obtain the average planning time of the algorithm(as shown in Tab.2). Green In Tab.3, the planning time of 20 times of the AFCS algorithm is stable between 3.904 s-5.2919 s, and the planning time fluctuates little in the green sphere environment. However, the 20 times path planning time of tRRT algorithm fluctuates from 4.4478s to 46.7573s.Therefore, the AFCS algorithm not only has high planning efficiency, but also has good stability. Path quality is an important factor to measure the planning efficiency of the algorithm. Path cost and the number of path nodes are important performances of path quality. In this paper, the sum of distance between nodes on the path is taken as the path cost. In this paper, the path cost obtained by running the AFCS algorithm and tRRT algorithm 5 times in two environments is shown in Fig. 14.Fig. 14 the path cost obtained of the AFCS algorithm and tRRT algorithm in two environmentsIn Fig. 14, the path cost of the AFCS algorithm is about 20% smaller than that of the tRRT algorithm in the red sphere environment. In a more complex environment (green sphere environment), the path cost of the adaptive field co-sampling algorithm is about 50% less than that of the tRRT algorithm, which indicates that the path of the adaptive field co-sampling algorithm is shorter. After running the algorithm 5 times, the path cost float of the AFCS algorithm is smaller than that of the tRRT algorithm, which indicates that the AFCS algorithm has better stability.In this paper, the AFCS algorithm and tRRT algorithm were run five times respectively in two environments, and the number of path nodes was obtained, as shown in Fig. 15.Fig. 15 The number of path nodes of the AFCS algorithm and tRRT algorithmIn Fig. 15, the number of path nodes planned by the AFCS algorithm for five times is also smaller than that of the tRRT algorithm in the red sphere environment, and the number of nodes decreases within a range of 2%-5%. In the green sphere environment, the advantage of the adaptive field co-sampling algorithm is more obvious, and the reduction interval of the number of path nodes for 5 times is 21.1%-50%. For different complex environments, the number of path nodes of the AFCS algorithm fluctuates little, ranging from 1.4% to 5.4%. In order to verify the practicability of the algorithm, the path planning simulation experiment is carried out combining the AFCS algorithm with ROAKE industrial robot XB20 in the green sphere environment. The whole system of the path planning experiment is composed of computer, ROKAE industrial robot XB20 and ZED2 depth camera. Among them, the communication and logical relationship of the three hardware devices are shown in Fig. 16.Fig. 16 The communication and logical relationship of the three hardware devices Based on the above hardware devices, the experimental flow is shown in Fig. 18.The whole process of the experiment consisted of three parts(as shown in Fig. 17).The first one is obstacle recognition based on ZED2 depth camera, which mainly obtains the position of obstacles for path planning of industrial robots.The second is path planning based on adaptive field cooperation, which mainly obtains collision-free paths of industrial robots.The third is to control the operation of industrial robots based on collision-free paths. This article studies the path planning of industrial robots from the unique perspective of improving the intelligence of path planning algorithm. And an AFCS algorithm with strong environmental adaptability is proposed. It uses the traditional RRT algorithm as the main framework to realize path planning. For the disadvantages of redundant sampling and low efficiency of the traditional RRT algorithm, this paper designs an optimal sampling strategy and improves the node expansion stage. The optimal sampling strategy not only solves the problem of redundant sampling, but also improves the quality of sampling points. The optimal sampling strategy is also beneficial to improve the path optimality. In the expansion stage, this paper firstly improves the traditional APF algorithm, and then introduces it to the node expansion stage of the traditional RRT algorithm. This approach provides a theoretical basis for the generation of new nodes and improves the overall efficiency of the algorithm. More importantly, compared with other algorithms, the efficiency and practicability of the AFCS algorithm are improved, and the adaptability to the environment is significantly improved in the path planning of industrial robots. This approach not only provides ideas for the intelligent development of path planning algorithms, but also provides guarantees for the intelligent development of industrial robots.