Path Planning of Continuum Robot Based on Path Fitting

,


Introduction
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. erefore, a device that can replace manual inspection is needed [1]. 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]. erefore, it is more flexible, safe, and suitable for aircraft fuel tank inspection tasks [4]. At present, many researchers have explored the application of Continuum Robots in human surgery [5], the maintenance of gas turbine engines [6], and the maintenance of nuclear power plants [7].
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 [8].
e path planning algorithms of traditional robots have been quite mature, such as A * [9], the artificial potential field method [10], RRT [11], ant colony [12], and particle swarm optimization [13], 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. erefore, 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. erefore, 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. erefore, 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 [16], 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 [17]. is 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 [18]. Godage et al. installed sensors on the joints of Continuum Robots to sense obstacles and achieve obstacle avoidance [19]. 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 [20]. 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 [21]. 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 [4]. However, it can only be applied to specific obstacle environments and cannot be effectively solved for the problems of compartments and multiple compartments.
is 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. e structure of this paper is as follows: e first part firstly introduces the kinematic model of Continuum Robot [22] and the working environment of Continuum Robot. e second part introduces the path planning method of Continuum Robot. e third part designs the path planning experiment of Continuum Robot and carries out the simulation verification.

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. e 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, singlejoint length L, bending angle θ, and rotation angle φ. en, the joint length of the ith joint is L, the bending angle θ i is the central angle of arc o i o i+1 ⌢ , and rotation angle φ i is the angle between plane o i ′ o i o i+1 and plane x i o i z i . According to the geometric relation, the coordinate system i { } of the first joint of the Continuum Robot, namely, coordinate system x i y i z i , and the coordinate system i + 1 { } of the end joint of the Continuum Robot, namely, coordinate system x i+1 y i+1 z i+1 , can be known. e transformation between them can be represented by the homogeneous transformation matrix (HTM), which is a 4 by 4 matrix, shown as follows: where i � 1, 2, 3..., θ i ∈ ( 0, π ], φ i ∈ [0, 2π], Trans (L, θ i , φ i ) represents the translation matrix, and R Z (φ i ), R Y (θ i ), and R Z (−φ i ) represent the coordinate transformation matrix around +Z, +Y, and +Z, respectively. R i is the rotation matrix, which is a 3 by 3 matrix, which can be expressed by formula (2). P i 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 n { } of joint segment n and coordinate system 1 { } of joint segment 1 can be obtained as follows:

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 x y y y z y , 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. e 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. e 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. e centers of spherical obstacles are set as p s1 � (20, 20, 50), p s2 � (50, 0, 30), and p s3 � (120, −30, 70), and the radius of the spherical obstacle is set as r s � 4 cm, respectively. e direction vector of a set of tubular obstacles is set as v 1 � (0, 1, 0), the radius of the tubular obstacle is set as r p1 � 2 cm, and the starting point is p p1 � (20, −50, 30), p p2 � (20, −50, 60), p p3 � (20, −50, 80), p p4 � (80, −50, 80), p p5 � (80, −50, 60), and p p6 � (80, −50, 80), respectively. e length is equal to length1 � 100 cm, e direction vector of another set of tubular obstacles is set as v 2 � (0, 0, 1), the radius of the tubular obstacle is set as r p2 � 2 cm, and the starting point is p p7 � (20, −40, 0), p p8 � (20, −30, 0), p p9 � (20, 10, 0), p p10 � (80, 40, 0), p p11 � (80, −30, 0), and p p12 � (80, 10, 0), respectively. e 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. e following assumptions are proposed.

Assumption 1.
e aircraft fuel tank is taken as the working environment of the Continuum Robot. e 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.
e target point of the path of the Continuum Robot is T point , and the distance between the end point (x, y, z) of the path of the Continuum Robot and the target point (x target , y target , z target ) is within the error range E. Assuming that the end joint curve of Continuum Robot can be expressed as function f, the following formula can be expressed: Assumption 3. e working environment of the Continuum Robot is known, and the obstacles are spherical and tubular. p p � (x pobs , y pobs , z pobs ) represents the starting point of the center axis of the pipe obstacle, r p represents the radius of the tubular obstacle, v represents the direction of the tubular obstacle, and length represents the length of the tubular obstacle. p s � (x sobs , y sobs , z sobs ) represents the center point of the spherical obstacle, r sobs represents the radius of the spherical obstacle, and the spherical obstacle can be represented as follows:

Introduction of the Path Planning Method of the Continuum Robot
To complete the path planning, start point S and end point T are set in the workspace of the Continuum Robot according to the modeling environment. Here, we define the starting point as S point and the ending point as T point , 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 [4], 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 S point to T point can be obtained as P � X new0 � S point , X new1 , X new2 , X new3 , . . . , X newn � T point }, en, 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. e entry direction of the Continuum Robot is perpendicular to the inlet direction of the aircraft fuel tank. e first joint fitting was performed within the range of singlejoint accessibility. en, 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. First, we take S point as the root node of the tree, take random samples in space, get random point X randi , then find the closest tree node, and increase the distance S from the tree node to the direction of random point X randi . If no obstacle is encountered, a new tree node X newi is obtained; otherwise, the growth of the tree node is ended. en, 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. e 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. e 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 RRTalgorithm does not meet the kinematic path of the Continuum Robot, and joint fitting needs to be carried out on the discrete path. Figure 7(a), the fuel tank coordinate system is x y y y z y , the first coordinate system of joint i { } is x i y i z i , and the terminal coordinate system of joint i + 1 { } is x i+1 y i+1 z i+1 . 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(

Joint Fitting Algorithm. As shown in
where θ is the bending angle of the fitted joint, φ is the rotation angle of the fitted joint, and O i+1 ∈ P is the end point of the fitting joint, which is one of the discrete points of the feasible path. O i+1 (1) and O i+1 (2) represent the x and y 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). en, 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). en, 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). en, 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. en, 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).
��������� �→ are shown in Figure 7(b), respectively. O i+1 ′ M i+1 is the perpendicular bisector of O i+1 D, φ is the rotation angle of the fitting joint, D ∈ P is the discrete point the fitting joint passes, which is a point in the discrete point of the feasible path, and D(1) and D (2) represent the x and y coordinates of the discrete point, respectively.

Obstacle Avoidance Strategy.
e 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 P i3 passes through obstacles, en, based on the original parameters, the joint parameters (bending angle θ and rotation angle φ) are fine tuned to avoid obstacles and a new matching path P i3 ′ 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).

e Experiment Design.
In this experiment, the starting point is set as S point � (0, 0, 0), the growth step of RRTalgorithm is set as S � 5 cm, and the expansion radius of the spherical obstacle and the tubular obstacle are set as r se � 2 cm and r pe � 0.5 cm, respectively. e error of the distance from the target point is set as E � 5 cm. en 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 P target1 � (115, 48, 2), the compartment entrance point P target2 � (115, 48, 38), and the top target point P target3 � (115, 48, 98), and then, the path planning experiment was carried out. e experimental results are shown in Figures 9 and 10. Figure 9 shows the whole path planning process with P target1 as the target point, Figures 10(a) and 10(b) show the schematic diagram of path planning results when P target2 and P target3 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 S point � (0, 0, 0) as the aircraft fuel tank entrance, S � 5 cm is the step growth of RRT algorithm, and the error of the distance from the target point is set as E � 5 cm. e bottom target points are set as P target1 � (10, 10, 30), P target2 � (40, 10, 30), and P target3 � (88, 10, 30) for path planning. e 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. e setting of the target point is the same as the barrier-free space, and the path planning is carried out.      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. e obstacle setting remains the same as (3), and the target point is set as P target4 � (115, 78, 88). e experimental results are shown in Figure 12(d), and the experimental process data are recorded in Table 4.

Result Analysis.
e abovementioned simulation results show that the path planning task of the Continuum Robot can be completed by this algorithm. e algorithm is suitable  for barrier-free space, obstacle space, compartment, and multicompartment environment. e feasibility and adaptability of the algorithm are demonstrated.

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. en, 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.
irdly, it is adaptable. e path planning of Continuum Robot in this algorithm is fitted on the basis of existing discrete point paths.
en, only the path expansion of discrete points can be extended for path planning. e motion of the Continuum Robot needs to strictly abide by its kinematic model. e 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.

Data Availability
e data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest
e authors declare that they have no conflicts of interest.