Research Article | Open Access
Guochen Niu, Yunxiao Zhang, Wenshuai Li, "Path Planning of Continuum Robot Based on Path Fitting", Journal of Control Science and Engineering, vol. 2020, Article ID 8826749, 11 pages, 2020. https://doi.org/10.1155/2020/8826749
Path Planning of Continuum Robot Based on Path Fitting
The Continuum Robot has a multiredundant dof structure, which is extremely advantageous in the unstructured environment, and can complete such tasks as aircraft fuel tank inspection. However, due to its complex kinematics and coupling of joint motion, its motion path planning is also a challenging task. In this paper, a path planning method for Continuum Robot based on an equal curvature model in an aircraft fuel tank environment is proposed. Considering the complexity of calculation and the structural characteristics of Continuum Robot, a feasible obstacle avoidance discrete path is obtained by using the improved RRT algorithm. Then, joint fitting is performed on the existing discrete path according to the kinematic model of Continuum Robot, joint obstacle avoidance was conducted in the process of fitting, and finally, a motion path suitable for the Continuum Robot was selected. A reasonable experiment is designed based on MATLAB, and simulation and analysis results demonstrate excellent performance of this method and feasibility of path planning.
In the fuel tank of civil aviation aircraft, there are a large number of obstacles such as tubing, diaphragm, and stringers, as well as a large number of flammable and explosive gases. It is a complex and narrow environment with complicated structure, high labor intensity, and potential safety hazards. Therefore, a device that can replace manual inspection is needed . Compared with the traditional rigid connecting rod robot, the Continuum Robot has the characteristics of multiple redundant degrees of freedom and the separation of the driver and controller, which can theoretically reach any point in space [2, 3]. Therefore, it is more flexible, safe, and suitable for aircraft fuel tank inspection tasks . At present, many researchers have explored the application of Continuum Robots in human surgery , the maintenance of gas turbine engines , and the maintenance of nuclear power plants .
In the process of robot inspection, path planning is often needed first. Because of the characteristics of multiple redundant degrees of freedom and joint coupling, path planning of Continuum Robot is also a challenging task . The path planning algorithms of traditional robots have been quite mature, such as A∗ , the artificial potential field method , RRT , ant colony , and particle swarm optimization , which can solve most robot path planning problems. However, most of the paths planned by the abovementioned methods are discrete point paths, which do not accord with the characteristics of Continuum Robots. Therefore, the traditional path planning cannot be directly applied to the path planning of Continuum Robots, and it needs to be fitted on the original path. Compared with other path planning algorithms, RRT algorithm is easy to restrict the growth direction of the path and has the characteristics of short time, so it is easier to generate the discrete path suitable for Continuum Robot fitting. Therefore, RRT algorithm is selected as the method to generate the discrete path in this paper.
As for the path planning of Continuum Robots, researchers have proposed many methods. Just like the path planning of general robots, Continuum Robots can also use inverse kinematics to solve the path. Godage and Walker proposed general modal dual Quaternion (DQ) kinematics for multisection continuum arms and solved the spatial inverse kinematics of the three-joint Continuum Robot by using this kinematics [14, 15]. However, the solution of the inverse kinematics could not avoid the problem of complex operation, and the solution path of the inverse kinematics would become more complicated with the increase of joints. Therefore, many researchers have also proposed the path solution method based on forward kinematics. Chirikjian and Burdick proposed the path planning of hyper-redundant manipulators in two-dimensional space based on geometric analysis , but the path planning in two-dimensional space cannot give full play to the advantages of Continuum Robots. Li and Xiao proposed a real-time collision detection algorithm based on analytical intersection checking with nonconvex primitives to realize the path planning of Continuum Robots . This paper focuses on the discussion of the path planning of Continuum Robot joints around objects when it grabs the object, which is not applicable to the path planning of general Continuum Robots. Torres et al. proposed a precalculated roadmap of collision-free robot configurations based on a description of the anatomical obstacles and adjusted the attitude of the Continuum Robot through tip perception to achieve automatic collision avoidance of the concentric tube robot . Godage et al. installed sensors on the joints of Continuum Robots to sense obstacles and achieve obstacle avoidance . However, the abovementioned two methods need to use other sensors to detect and avoid obstacles, and there are strict requirements on the installation location of sensors; Yu et al. proposed a path planning method to avoid collision of continuous multijoint robots. When the planned path collides with obstacles, joint attitude is adjusted to make Continuum Robots avoid obstacles . However, this algorithm only carries out the strategy of avoiding obstacles when there are obstacles in the known path and does not discuss how to generate a feasible path. Deng et al. proposed a path planning method based on spatial configuration diagram, discretized the workspace into unit cubes, and used Bellman–Ford algorithm to find and generate smooth paths . But, this algorithm may lead to no solution, it is and easy to make the path excessive bending, resulting in wasted joints. Researchers have proposed many algorithms for path planning of Continuum Robots, but they are not directly applicable to aircraft fuel tank inspection environments; the project team proposed a method of dimension reduction and region clipping to simplify path planning and projected the obstacle space into a two-dimensional plane, simplifying the obstacle avoidance task . However, it can only be applied to specific obstacle environments and cannot be effectively solved for the problems of compartments and multiple compartments.
This paper proposes a path planning method for Continuum Robots, which can avoid obstacles and complete path planning for Continuum Robots in complex fuel tank environment and can be used for inspection of aircraft fuel tank compartments and multiple compartments.
The structure of this paper is as follows: The first part firstly introduces the kinematic model of Continuum Robot  and the working environment of Continuum Robot. The second part introduces the path planning method of Continuum Robot. The third part designs the path planning experiment of Continuum Robot and carries out the simulation verification.
2. Introduction to Continuum Robots
2.1. Kinematic Model of the Continuum Robot
Before the path planning of the Continuum Robot, the kinematic model of the Continuum Robot must be established. Depending on its kinematic model, the path suitable for Continuum Robot structure can be planned. It is assumed that the Continuum Robot model is an arc model of equal curvature, and the length of each joint is constant.
The kinematic model of the Continuum Robot with a single-joint segment is shown in figure (Figure 1). It is simplified as a continuous smooth curve of equal curvature, and three variables of Continuum Robot are defined, single-joint length , bending angle , and rotation angle . Then, the joint length of the ith joint is , the bending angle is the central angle of arc , and rotation angle is the angle between plane and plane .
According to the geometric relation, the coordinate system of the first joint of the Continuum Robot, namely, coordinate system , and the coordinate system of the end joint of the Continuum Robot, namely, coordinate system , can be known. The transformation between them can be represented by the homogeneous transformation matrix (HTM), which is a 4 by 4 matrix, shown as follows:where , , , represents the translation matrix, and , , and represent the coordinate transformation matrix around +Z, +Y, and +Z, respectively. is the rotation matrix, which is a 3 by 3 matrix, which can be expressed by formula (2). is the position vector, which is a 3 by 1 matrix, which can be expressed by Formula (3).
Each joint is an arc model of equal curvature, and then, according to the coordinate transformation formula, the homogeneous transformation matrix between the coordinate system of joint segment and coordinate system of joint segment 1 can be obtained as follows:
2.2. Continuum Robot Working Environment
Here, the fuel tank environment is taken as the working environment of the Continuum Robot. Figure 2 shows part of the environment of the aircraft fuel tank, and it can be seen that there are a large number of oil pipelines and stringers in the aircraft fuel tank environment. Here, the obstacles in the working environment of the Continuum Robot are simplified to be tubular obstacles and spherical obstacles.
During fuel tank inspection, it is necessary to enter through the fuel tank entrance. Here, the direction of aircraft fuel tank entrance, namely, the z-axis of fuel tank coordinate system , is taken as the direction of Continuum Robot entry, as shown in Figure 3. During the tank inspection, there is a situation that the tank enters the compartment from the single compartment, as shown in Figure 3. The tank entry point is defined as the origin of the tank coordinate system, and the first coordinate system of the first joint of the Continuum Robot is defined as the joint space coordinate system.
The tank environment is modeled, the tank environment is simplified as shown in Figure 4, which is 300 cm long (X), 200 cm wide (Y), and 100 cm high (Z), and tubular obstacles and spherical obstacles are set in the space. In order to verify the obstacle avoidance performance of the algorithm, nine tubular obstacles are added near the entrance and near the entrance of the compartment to form an approximate network structure, and three spherical obstacles are added in the possible path. The centers of spherical obstacles are set as , , and , and the radius of the spherical obstacle is set as , respectively. The direction vector of a set of tubular obstacles is set as , the radius of the tubular obstacle is set as , and the starting point is , , , , , and , respectively. The length is equal to , The direction vector of another set of tubular obstacles is set as , the radius of the tubular obstacle is set as , and the starting point is , , , , , and , respectively. The length is equal to 100 cm.
Here, the working environment, initial state of the Continuum Robot, and the target point to be reached are given. The following assumptions are proposed.
Assumption 1. The aircraft fuel tank is taken as the working environment of the Continuum Robot. The initial state of the Continuum Robot is directly below the entrance of the aircraft fuel tank, and it enters the aircraft fuel tank in a direction perpendicular to the entrance, as shown in Figure 3.
Assumption 2. The target point of the path of the Continuum Robot is , and the distance between the end point of the path of the Continuum Robot and the target point is within the error range . Assuming that the end joint curve of Continuum Robot can be expressed as function , the following formula can be expressed:
Assumption 3. The working environment of the Continuum Robot is known, and the obstacles are spherical and tubular. represents the starting point of the center axis of the pipe obstacle, represents the radius of the tubular obstacle, represents the direction of the tubular obstacle, and represents the length of the tubular obstacle. represents the center point of the spherical obstacle, represents the radius of the spherical obstacle, and the spherical obstacle can be represented as follows:
3. Introduction of the Path Planning Method of the Continuum Robot
To complete the path planning, start point and end point are set in the workspace of the Continuum Robot according to the modeling environment. Here, we define the starting point as and the ending point as , and then, we can do path planning just like the general path planning problem. Because of the special structure of the Continuum Robot, each joint is continuous , which cannot be bent suddenly. Here, the RRT path planning algorithm, which is easy to constrain the path form, is chosen for path planning, and then, the feasible path of a sequence of points from to can be obtained as , Then, according to the kinematics of the Continuum Robot, joint search is performed on a series of discrete path points, which turns the aimless forward path search into path search on the existing discrete obstacle avoidance path.
The entry direction of the Continuum Robot is perpendicular to the inlet direction of the aircraft fuel tank. The first joint fitting was performed within the range of single-joint accessibility. Then, the next joint fitting is carried out on the basis of this joint, and obstacles are effectively avoided in the fitting process, until it finally reaches the point near the target, where formula (4) is satisfied, and once path planning is completed.
3.1. Fast Expansion Random Tree (RRT) Algorithm
The process of RRT algorithm starts from the starting point set in the space, through random sampling in the space, and it keeps growing to the sampling point and adding new nodes . When the new node is the target node, a path can be generated from the starting point to the target point .
First, we take as the root node of the tree, take random samples in space, get random point , then find the closest tree node, and increase the distance from the tree node to the direction of random point . If no obstacle is encountered, a new tree node is obtained; otherwise, the growth of the tree node is ended. Then, the algorithm will be resampled randomly in the space and continue to grow until the new tree node is the target node, as shown in Figure 5.
According to Assumption 1, the Continuum Robot takes the entry direction perpendicular to the inlet of the aircraft’s fuel tank as the entry direction, and the RRT algorithm is improved to give a direction vector perpendicular to the inlet of the aircraft fuel tank near the inlet. In order to better realize the obstacle avoidance strategy, the obstacles are treated with expansion when path planning is carried out using RRT algorithm.
The path planned by the RRT algorithm in the tank environment without obstacles is shown in Figure 6. We take the path growth length as S = 5 cm, where the starting and ending points are represented by pentacle symbols, and the blue point is the random sampling point in the space. The black and red lines are the random trees generated by the RRT algorithm, where red lines are the paths that can reach the target point, as shown in figure (Figure 6).
According to the kinematic model of the Continuum Robot, the path of discrete points obtained by RRT algorithm does not meet the kinematic path of the Continuum Robot, and joint fitting needs to be carried out on the discrete path.
3.2. Joint Fitting Algorithm
As shown in Figure 7(a), the fuel tank coordinate system is , the first coordinate system of joint is , and the terminal coordinate system of joint is . According to the kinematic model of the Continuum Robot, the set of discrete path points in the space of single joint can be obtained. Each discrete path point in the set has a unique single-joint fitting path, so a single-joint fitting path set can be obtained. As shown in Figure 7(a), only a unique fitting path exists for discrete point . The corresponding joint parameters can be obtained.where is the bending angle of the fitted joint, is the rotation angle of the fitted joint, and is the end point of the fitting joint, which is one of the discrete points of the feasible path. and represent the and coordinates of the end points, respectively.
For the fitting of the middle joint, firstly, we remove the discrete path point before the joint node (i node). Then, the homogeneous transformation matrix is used to convert the remaining discrete path points to the coordinate system of the i + 1 joint, and a new set of discrete path points is obtained, as shown in Figure 7(c). Then, single-joint fitting is carried out in this coordinate system. According to the kinematic model of the Continuum Robot, the only fitting path passing through the point can be obtained for each discrete path point in a single-joint space, as shown in Figure 7(b). Then, a set of joint fitting paths can be obtained, as shown in Figure 7(c). According to formula (8), the parameters of each matching joint can be obtained. Then, according to the direction of the coordinate system at the end of the joint, the better single-joint path is selected, as shown in Figure 7(d).where vectors and are shown in Figure 7(b), respectively. is the perpendicular bisector of , is the rotation angle of the fitting joint, is the discrete point the fitting joint passes, which is a point in the discrete point of the feasible path, and and represent the and coordinates of the discrete point, respectively.
3.3. Obstacle Avoidance Strategy
The discrete point path obtained by RRT algorithm has some obstacle avoidance ability, but there is still the case that the joint passes through the obstacle during the fitting process, so it is necessary to design the obstacle avoidance strategy to avoid the collision.
When the matching joint meets the obstacle, the joint parameters obtained by formula (6) or formula (7) are fine tuned to avoid the obstacle. As shown in Figure 8(a), joint fitting path passes through obstacles, Then, based on the original parameters, the joint parameters (bending angle and rotation angle ) are fine tuned to avoid obstacles and a new matching path is obtained. When the obstacle cannot be avoided by fine tuning the parameters of the local joint, the joint parameters of the previous joint can be fine tuned or both can be adjusted to avoid obstacles, as shown in Figure 8(b).
4. Simulation Experiment
4.1. The Experiment Design
In this experiment, the starting point is set as , the growth step of RRT algorithm is set as , and the expansion radius of the spherical obstacle and the tubular obstacle are set as and , respectively. The error of the distance from the target point is set as E = 5 cm. Then path planning is carried out by MATLAB, computer configuration Intel(R) Core(TM) i7-4510u 2.60 GHz.(1)Path planning is carried out in an environment free of obstacles, the end point is set as the bottom target point , the compartment entrance point , and the top target point , and then, the path planning experiment was carried out. The experimental results are shown in Figures 9 and 10. Figure 9 shows the whole path planning process with as the target point, Figures 10(a) and 10(b) show the schematic diagram of path planning results when and are the target points, and Table 1 records the experimental process data.(2)Path planning is carried out in a single cabin barrier-free environment. We set the starting point as the aircraft fuel tank entrance, is the step growth of RRT algorithm, and the error of the distance from the target point is set as E = 5 cm. The bottom target points are set as , , and for path planning. The experimental results are shown in Figure 11, and the data for path planning based on traversal search and path fitting are recorded in Table 2. Sign A represents path fitting path planning, and sign B represents traversal search path planning. It can be seen from the experimental results that the path planning of single cabin barrier-free environment can be realized by this path planning algorithm. Compared with spatial traversal search, it has more time advantage with the increase of joint number.(3)Path planning is carried out in an environment with obstacles. Obstacles in this space are set up as in Section 2.2. The setting of the target point is the same as the barrier-free space, and the path planning is carried out. The experimental results are shown in Figures 12(a) and 12(c), and the experimental process data are recorded in Table 3.(4)Path planning is carried out in a compartment environment with obstacles. The obstacle setting remains the same as (3), and the target point is set as . The experimental results are shown in Figure 12(d), and the experimental process data are recorded in Table 4.
The rising distance represents the rising distance between the base coordinate system of the Continuum Robot and the fuel tank coordinate system.
4.2. Result Analysis
The abovementioned simulation results show that the path planning task of the Continuum Robot can be completed by this algorithm. The algorithm is suitable for barrier-free space, obstacle space, compartment, and multicompartment environment. The feasibility and adaptability of the algorithm are demonstrated.
5. Conclusions and Prospect
In this paper, the aircraft fuel tank environment is used as the simulation environment for Continuum Robot operation, and tubular obstacles and spherical obstacles are added into the environment. RRT path planning algorithm-optimized growth rules are proposed to generate discrete point paths suitable for path fitting of Continuum Robots. Then, according to the kinematic model of the Continuum Robot, the motion path of the Continuum Robot is obtained through joint fitting, the feasibility of the algorithm is verified by simulation, and the simulation is carried out.
Firstly, based on the path search algorithm, it can be known that the complexity of the algorithm is positively correlated with the density and number of discrete path points obtained by RRT. In addition, compared with other path search algorithms, the search time will not increase sharply with the increase of the number of joints, and this algorithm has a time advantage when more joints are needed.
Secondly, it has the advantage of avoiding obstacles. Since the discrete point path planned by RRT path planning algorithm has the characteristic of obstacle avoidance, path fitting based on the discrete point path can reduce the burden of obstacle avoidance strategy.
Thirdly, it is adaptable. The path planning of Continuum Robot in this algorithm is fitted on the basis of existing discrete point paths. Then, only the path expansion of discrete points can be extended for path planning.
The motion of the Continuum Robot needs to strictly abide by its kinematic model. The path of discrete points planned by RRT algorithm is not the most suitable path fitting for the Continuum Robot in this environment, and there is still a large optimization space.
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
The authors would like to thank their colleagues in the Robotics Institute of Civil Aviation University of China for their significant contributions. This work was funded by the Tianjin Research Program of Application Foundation and Advanced Technology #14JCQNJC04400 and the Tianjin Science and Technology Support Program #17ZXHLGX00120.
- N. Guochen, W. Li, G. Qingji, and H. Dandan, “Path-tracking algorithm for aircraft fuel tank inspection robots,” International Journal of Advanced Robotic Systems, vol. 11, no. 5, p. 82, 2014.
- G. Robinson and J. B. C. Davies, “Continuum robots—a state of the art,” in Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C), IEEE, Detroit, MI, USA, May 1999.
- R. Buckingham, “Snake arm robots,” Industrial Robot: An International Journal, vol. 29, no. 3, pp. 242–245, 2002.
- G. Niu, Z. Zheng, and Q. Gao, “Collision free path planning based on region clipping for aircraft fuel tank inspection robot,” in Proceedings of the 2014 IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, China, June 2014.
- H. B. Gilbert, D. C. Rucker, and R. J. Webster, “Concentric tube robots: the state of the art and future directions,” in Robotics Research, Springer International Publishing, Berlin, Germany, 2016.
- J. Kell, D. A. Axinte, and D. Palmer, “Development of a slender continuum robotic system for on-wing inspection/repair of gas turbine engines,” Robotics and Computer-Integrated Manufacturing, vol. 44, pp. 218–229, 2017.
- R. Buckingham and A. Graham, “Nuclear snake–arm robots,” Industrial Robot: An International Journal, vol. 39, no. 1, pp. 6–11, 2012.
- L. Sun, H. Hu, and M. Li, “A review on continuum robot,” Robot, vol. 32, no. 5, pp. 688–694, 2010.
- P. Hart, N. Nilsson, and B. Raphael, “A formal basis for the heuristic determination of minimum cost paths,” IEEE Transactions on Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, 1968.
- O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” in Autonomous Robot Vehicles, pp. 396–404, Springer, New York, NY, USA, 1986.
- C. Moon and W. Chung, “Kinodynamic planner dual-tree RRT (DT-RRT) for two-wheeled mobile robots using the rapidly exploring random tree,” IEEE Transactions on Industrial Electronics, vol. 62, no. 2, pp. 1080–1090, 2014.
- M. Dorigo, V. Maniezzo, and A. Colorni, “Ant system: optimization by a colony of cooperating agents,” IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics), vol. 26, no. 1, pp. 29–41, 1996.
- Y. Hao, W. Zu, and Y. Zhao, “Real-time obstacle avoidance method based on polar coordination particle swarm optimization in dynamic environment,” in Proceedings of the 2007 2nd IEEE Conference On Industrial Electronics and Applications, pp. 1612–1617, IEEE, Harbin, China, May 2007.
- I. S. Godage and I. D. Walker, “Dual Quaternion based modal kinematics for multisection continuum arms,” in Proceedings of the 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 1416–1422, IEEE, Seattle, WA, USA, May 2015.
- I. S. Godage, G. A. Medrano-Cerda, D. T. Branson, E. Guglielmino, and D. G. Caldwell, “Modal kinematics for multisection continuum arms,” Bioinspiration & Biomimetics, vol. 10, no. 3, Article ID 035002, 2015.
- G. S. Chirikjian and J. W. Burdick, “An obstacle avoidance algorithm for hyper-redundant manipulators,” in Proceedings of the IEEE International Conference on Robotics and Automation, pp. 625–631, IEEE, Cincinnati, OH, USA, May 1990.
- J. Li and J. Xiao, “An efficient algorithm for real time collision detection involving a continuum manipulator with multiple uniform-curvature sections,” Robotica, vol. 34, no. 7, pp. 1566–1586, 2016.
- L. G. Torres, A. Kuntz, H. B. Gilbert et al., “A motion planning approach to automatic obstacle avoidance during concentric tube robot teleoperation[C]//2015 IEEE International Conference on Robotics and Automation (ICRA),” IEEE, 2015.
- I. S. Godage, D. T. Branson, E. Guglielmino et al., “Path planning for multisection continuum arms,” in Proceedings of the International Conference on Mechatronics & Automation, IEEE, Chengdu, China, August 2012.
- X. Yu, X. Wang, D. Meng et al., “Collision free path planning for multi-section continuum manipulators based on a modal method,” in Proceedings of the 2018 IEEE 8th annual international conference on CYBER Technology in automation, control, and intelligent systems (CYBER), pp. 236–242, IEEE, Tianjin, China, July 2018.
- J. Deng, B. H. Meng, I. Kanj et al., “Near-optimal smooth path planning for multisection continuum arms,” in Proceedings of the 2019 2nd IEEE international conference on soft robotics (RoboSoft), pp. 416–421, IEEE, Seoul, Republic of Korea, April 2019.
- Q. Gao, L. Wang, G. C. Niu et al., “Path planning for continuum robot based on target guided angle,” Journal of Beijing University of Aeronautics and Astronautics, vol. 39, no. 11, pp. 1486–1490, 2013.
Copyright © 2020 Guochen Niu et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.