# Path Planning of Continuum Robot Based on Path Fitting

Authors: Guochen Niu; Yunxiao Zhang; Wenshuai Li
Journal: Journal of Control Science and Engineering
 (2020)

---

## Abstract

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.


---

## Body

## 1. 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. Therefore, 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]. Therefore, 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]. The 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. 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 [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]. 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 [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.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 [22] 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 (Figure1). It is simplified as a continuous smooth curve of equal curvature, and three variables of Continuum Robot are defined, single-joint length L, bending angle θ, and rotation angle φ. Then, the joint length of the ith joint is L, the bending angle θi is the central angle of arc oioi+1⌢, and rotation angle φi is the angle between plane oi′oioi+1 and plane xioizi.Figure 1
Single-joint kinematics model.According to the geometric relation, the coordinate systemi of the first joint of the Continuum Robot, namely, coordinate system xiyizi, and the coordinate system i+1 of the end joint of the Continuum Robot, namely, coordinate system xi+1yi+1zi+1, 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:(1)Ti=TransL,θi,φiRZφiRYθiRZ−φi,=RiPi01,where i=1,2,3..., θi∈0,π, φi∈0,2π, TransL,θi,φi represents the translation matrix, and RZφi, RYθi, and RZ−φi represent the coordinate transformation matrix around +Z, +Y, and +Z, respectively. Ri is the rotation matrix, which is a 3 by 3 matrix, which can be expressed by formula (2). Pi is the position vector, which is a 3 by 1 matrix, which can be expressed by Formula (3).(2)Ri=cθic2φi+s2φicθisφicφi−cφisφisθicφicθicφisφi−cφisφic2φi+s2φicθisθisφi−sθicφi−sθisφicθio,(3)Pi=Lθicosφi1−cθiLθisinφi1−cθiLθisinθiT.Each joint is an arc model of equal curvature, and then, according to the coordinate transformation formula, the homogeneous transformation matrix between the coordinate systemn of joint segment n and coordinate system 1 of joint segment 1 can be obtained as follows:(4)Tn+11=T21⋅T32⋯Tn+1n.


### 2.2. Continuum Robot Working Environment

Here, the fuel tank environment is taken as the working environment of the Continuum Robot. Figure2 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.Figure 2
Aircraft fuel tank part environment.During fuel tank inspection, it is necessary to enter through the fuel tank entrance. Here, the direction of aircraft fuel tank entrance, namely, thez-axis of fuel tank coordinate system xyyyzy, 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.Figure 3
Fuel tank inspection diagram.The tank environment is modeled, the tank environment is simplified as shown in Figure4, 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 ps1=20,20,50, ps2=50,0,30, and ps3=120,−30,70, and the radius of the spherical obstacle is set as rs=4cm, respectively. The direction vector of a set of tubular obstacles is set as v1=0,1,0, the radius of the tubular obstacle is set as rp1=2cm, and the starting point is pp1=20,−50,30, pp2=20,−50,60, pp3=20,−50,80, pp4=80,−50,80, pp5=80,−50,60, and pp6=80,−50,80, respectively. The length is equal to length1=100cm, The direction vector of another set of tubular obstacles is set as v2=0,0,1, the radius of the tubular obstacle is set as rp2=2cm, and the starting point is pp7=20,−40,0, pp8=20,−30,0, pp9=20,10,0, pp10=80,40,0, pp11=80,−30,0, and pp12=80,10,0, respectively. The length is equal to 100 cm.Figure 4
Aircraft fuel tank modeling diagram.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 Figure3.Assumption 2.
The target point of the path of the Continuum Robot isTpoint, and the distance between the end point x,y,z of the path of the Continuum Robot and the target point xtarget,ytarget,ztarget 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:(5)fTpoint=0,x−xtarget2+y−ytarget2+z−ztarget2≤E.Assumption 3.
The working environment of the Continuum Robot is known, and the obstacles are spherical and tubular.pp=xpobs,ypobs,zpobs represents the starting point of the center axis of the pipe obstacle, rp represents the radius of the tubular obstacle, v represents the direction of the tubular obstacle, and length represents the length of the tubular obstacle. ps=xsobs,ysobs,zsobs represents the center point of the spherical obstacle, rsobs represents the radius of the spherical obstacle, and the spherical obstacle can be represented as follows:(6)y−ysobs2+x−xsobs2+z−zsobs2<rsobs2.


## 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 (Figure1). It is simplified as a continuous smooth curve of equal curvature, and three variables of Continuum Robot are defined, single-joint length L, bending angle θ, and rotation angle φ. Then, the joint length of the ith joint is L, the bending angle θi is the central angle of arc oioi+1⌢, and rotation angle φi is the angle between plane oi′oioi+1 and plane xioizi.Figure 1
Single-joint kinematics model.According to the geometric relation, the coordinate systemi of the first joint of the Continuum Robot, namely, coordinate system xiyizi, and the coordinate system i+1 of the end joint of the Continuum Robot, namely, coordinate system xi+1yi+1zi+1, 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:(1)Ti=TransL,θi,φiRZφiRYθiRZ−φi,=RiPi01,where i=1,2,3..., θi∈0,π, φi∈0,2π, TransL,θi,φi represents the translation matrix, and RZφi, RYθi, and RZ−φi represent the coordinate transformation matrix around +Z, +Y, and +Z, respectively. Ri is the rotation matrix, which is a 3 by 3 matrix, which can be expressed by formula (2). Pi is the position vector, which is a 3 by 1 matrix, which can be expressed by Formula (3).(2)Ri=cθic2φi+s2φicθisφicφi−cφisφisθicφicθicφisφi−cφisφic2φi+s2φicθisθisφi−sθicφi−sθisφicθio,(3)Pi=Lθicosφi1−cθiLθisinφi1−cθiLθisinθiT.Each joint is an arc model of equal curvature, and then, according to the coordinate transformation formula, the homogeneous transformation matrix between the coordinate systemn of joint segment n and coordinate system 1 of joint segment 1 can be obtained as follows:(4)Tn+11=T21⋅T32⋯Tn+1n.


## 2.2. Continuum Robot Working Environment

Here, the fuel tank environment is taken as the working environment of the Continuum Robot. Figure2 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.Figure 2
Aircraft fuel tank part environment.During fuel tank inspection, it is necessary to enter through the fuel tank entrance. Here, the direction of aircraft fuel tank entrance, namely, thez-axis of fuel tank coordinate system xyyyzy, 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.Figure 3
Fuel tank inspection diagram.The tank environment is modeled, the tank environment is simplified as shown in Figure4, 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 ps1=20,20,50, ps2=50,0,30, and ps3=120,−30,70, and the radius of the spherical obstacle is set as rs=4cm, respectively. The direction vector of a set of tubular obstacles is set as v1=0,1,0, the radius of the tubular obstacle is set as rp1=2cm, and the starting point is pp1=20,−50,30, pp2=20,−50,60, pp3=20,−50,80, pp4=80,−50,80, pp5=80,−50,60, and pp6=80,−50,80, respectively. The length is equal to length1=100cm, The direction vector of another set of tubular obstacles is set as v2=0,0,1, the radius of the tubular obstacle is set as rp2=2cm, and the starting point is pp7=20,−40,0, pp8=20,−30,0, pp9=20,10,0, pp10=80,40,0, pp11=80,−30,0, and pp12=80,10,0, respectively. The length is equal to 100 cm.Figure 4
Aircraft fuel tank modeling diagram.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 Figure3.Assumption 2.
The target point of the path of the Continuum Robot isTpoint, and the distance between the end point x,y,z of the path of the Continuum Robot and the target point xtarget,ytarget,ztarget 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:(5)fTpoint=0,x−xtarget2+y−ytarget2+z−ztarget2≤E.Assumption 3.
The working environment of the Continuum Robot is known, and the obstacles are spherical and tubular.pp=xpobs,ypobs,zpobs represents the starting point of the center axis of the pipe obstacle, rp represents the radius of the tubular obstacle, v represents the direction of the tubular obstacle, and length represents the length of the tubular obstacle. ps=xsobs,ysobs,zsobs represents the center point of the spherical obstacle, rsobs represents the radius of the spherical obstacle, and the spherical obstacle can be represented as follows:(6)y−ysobs2+x−xsobs2+z−zsobs2<rsobs2.


## 3. Introduction of the Path Planning Method of the Continuum Robot

To complete the path planning, start pointS 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 Spoint and the ending point as Tpoint, 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 Spoint to Tpoint can be obtained as P=Xnew0=Spoint,Xnew1,Xnew2,Xnew3,…,Xnewn=Tpoint, 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 pointSpoint set in the space, through random sampling in the space, and it keeps growing to the sampling point and adding new nodes Xnewi. When the new node is the target node, a path can be generated from the starting point Spoint to the target point Tpoint.First, we takeSpoint as the root node of the tree, take random samples in space, get random point Xrandi, then find the closest tree node, and increase the distance S from the tree node to the direction of random point Xrandi. If no obstacle is encountered, a new tree node Xnewi 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.Figure 5
RRT algorithm diagram.According to Assumption1, 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 Figure6. 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).Figure 6
RRT path planning simulation diagram.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 Figure7(a), the fuel tank coordinate system is xyyyzy, the first coordinate system of joint i is xiyizi, and the terminal coordinate system of joint i+1 is xi+1yi+1zi+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(a), only a unique fitting path oioi+1⌢ exists for discrete point oi+1. The corresponding joint parameters can be obtained.(7)L/θ∗1−cosθ−sqrtOi+112+Oi+122=0φ=arctan2Oi+12,Oi+11,where θ is the bending angle of the fitted joint, φ is the rotation angle of the fitted joint, and Oi+1∈P is the end point of the fitting joint, which is one of the discrete points of the feasible path. Oi+11 and Oi+12 represent the x and y coordinates of the end points, respectively.Figure 7
Joint fitting schematic. (a) Path fitting of the first joint. (b) Path fitting of the middle joint. (c) Path fitting of the middle joint. (d) better path of the middle fitting joint.
(a)(b)(c)(d)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 thei + 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).(8)Oi+1D⟶∗Oi+1′Mi+1⟶=0φ=arctan2D2,O1,where vectors Oi+1D⟶ and Oi+1′Mi+1⟶ are shown in Figure 7(b), respectively. Oi+1′Mi+1 is the perpendicular bisector of Oi+1D, φ 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 D1 and D2 represent the x and y 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 Pi3 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 Pi3′ 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).Figure 8
Obstacle avoidance diagram. (a) Obstacle avoidance diagram 1. (b) Obstacle avoidance diagram 2.
(a)(b)


## 3.1. Fast Expansion Random Tree (RRT) Algorithm

The process of RRT algorithm starts from the starting pointSpoint set in the space, through random sampling in the space, and it keeps growing to the sampling point and adding new nodes Xnewi. When the new node is the target node, a path can be generated from the starting point Spoint to the target point Tpoint.First, we takeSpoint as the root node of the tree, take random samples in space, get random point Xrandi, then find the closest tree node, and increase the distance S from the tree node to the direction of random point Xrandi. If no obstacle is encountered, a new tree node Xnewi 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.Figure 5
RRT algorithm diagram.According to Assumption1, 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 Figure6. 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).Figure 6
RRT path planning simulation diagram.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 Figure7(a), the fuel tank coordinate system is xyyyzy, the first coordinate system of joint i is xiyizi, and the terminal coordinate system of joint i+1 is xi+1yi+1zi+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(a), only a unique fitting path oioi+1⌢ exists for discrete point oi+1. The corresponding joint parameters can be obtained.(7)L/θ∗1−cosθ−sqrtOi+112+Oi+122=0φ=arctan2Oi+12,Oi+11,where θ is the bending angle of the fitted joint, φ is the rotation angle of the fitted joint, and Oi+1∈P is the end point of the fitting joint, which is one of the discrete points of the feasible path. Oi+11 and Oi+12 represent the x and y coordinates of the end points, respectively.Figure 7
Joint fitting schematic. (a) Path fitting of the first joint. (b) Path fitting of the middle joint. (c) Path fitting of the middle joint. (d) better path of the middle fitting joint.
(a)(b)(c)(d)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 thei + 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).(8)Oi+1D⟶∗Oi+1′Mi+1⟶=0φ=arctan2D2,O1,where vectors Oi+1D⟶ and Oi+1′Mi+1⟶ are shown in Figure 7(b), respectively. Oi+1′Mi+1 is the perpendicular bisector of Oi+1D, φ 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 D1 and D2 represent the x and y 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 Pi3 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 Pi3′ 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).Figure 8
Obstacle avoidance diagram. (a) Obstacle avoidance diagram 1. (b) Obstacle avoidance diagram 2.
(a)(b)


## 4. Simulation Experiment

### 4.1. The Experiment Design

In this experiment, the starting point is set asSpoint=0,0,0, the growth step of RRT algorithm is set as S=5cm, and the expansion radius of the spherical obstacle and the tubular obstacle are set as rse=2cm and rpe=0.5cm, 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 pointPtarget1=115,48,2, the compartment entrance point Ptarget2=115,48,38, and the top target point Ptarget3=115,48,98, 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 Ptarget1 as the target point, Figures 10(a) and 10(b) show the schematic diagram of path planning results when Ptarget2 and Ptarget3 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 pointSpoint=0,0,0 as the aircraft fuel tank entrance, S=5cm 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 Ptarget1=10,10,30, Ptarget2=40,10,30, and Ptarget3=88,10,30 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 Section2.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 asPtarget4=115,78,88. The experimental results are shown in Figure 12(d), and the experimental process data are recorded in Table 4.Table 1
Barrier-free space path planning data.
Target pointBending angle (rad)Rotation angle (rad)Rising distance (cm)End pointError (cm)Time (s)(115, 48, 2){0.66, 1.24, 0.97, 1.71}{0.27, −0.15, 1.80, 0.27}182.34(114.61, 48.10, 1.01)1.0813.62(115, 48, 38){0.14, 1.45, 0.82, 0.66}{0.20, −0.13, 1.79, 0.75}153.80(115.19, 48.40, 37.88)0.4612.31(115, 48, 98){1.27, 0.14, 1.55, 0.85}{0.01, −1.31, 2.09, −1.89}185.16(115.15, 48.10, 98.41)0.4517.67The rising distance represents the rising distance between the base coordinate system of the Continuum Robot and the fuel tank coordinate system.Table 2
Traversal search and path fitting path planning data.
SignTarget pointBending angle (rad)Rotation angle (rad)Rising distance (cm)End pointError (cm)Time (s)A(10, 10, 30){0.58}{0.79}32.77(10, 10, 30)02.68B(10, 10, 30){0.57}{0.76}32.63(10.04, 9.45, 30)0.550.003A(40, 10, 30){0.08, 1.91}{0.32, 0.24}59.49(37.46, 9.39, 31.34)2.945.42B(40, 10, 30){0.50, 3.02}{0.00, 0.25}95.72(41.32, 8.25, 30.00)2.190.61A(88, 10, 30){0.13, 1.76, 0.77}{0.17, 0.05, 2.91}104.04(88.11, 10.03, 30.05)0.138.79B(88, 10, 30){0.19, 2.26, 0.57}{0.00, 0.06, 2.89}148.55(83.29, 9.42, 30.00)4.751218.54Table 3
Obstacle space path planning data.
Target pointBending angle (rad)Rotation angle (rad)Rising distance (cm)End pointError (cm)Time (s)(115, 48, 2){0.10, 1.72, 0.46, 1.27}{0.51, 0.15, 2.45, 0.85}154.35(115.03, 48.11, 1.81)0.2213.72(115, 48, 38){1.39, 0.50, 0.29}{0.07, 1.31, 1.38}150(115.19, 48.20, 37.98)0.2710.91(115, 48, 98){0.09, 1.45, 0.46, 1.78, 0.97}{0.40, −0.09, 0.95, 2.88, 0.82}154.56(115.03, 48.32, 98.16)0.3517.43(115, 78, 88){1.41, 0.10, 1.44, 1.37, 2.23}{0.07, −1.88, 1.29, 2.29, −2.67}250(114.87, 78.07, 87.96)0.1527.14Table 4
Obstacle space compartment path planning data.
Target pointBending angle (rad)Rotation angle (rad)Rising distance (cm)End pointError (cm)Time (s)(115, 78, 88){1.41, 0.10, 1.44, 1.37, 2.23}{0.07, −1.88, 1.29, 2.29, −2.67}250(114.87, 78.07, 87.96)0.1527.14Figure 9
Ptarget1=115,48,2 path planning process. (a) RRT discrete paths. (b) Fitting path. (c) The path that satisfies the endpoint condition. (d) The final path.
(a)(b)(c)(d)Figure 10
Barrier-free space path planning. (a)Ptarget2=115,48,38 barrier-free space path planning. (b) Ptarget3=115,48,98 barrier-free space path planning.
(a)(b)Figure 11
Traversal search and path fitting path planning comparison. (a)Ptarget1 path fitting path planning. (b) Ptarget2 path fitting path planning. (c) Ptarget3 path fitting path planning. (d) Ptarget1 traversal search path planning. (e) Ptarget2 traversal search path planning. (f) Ptarget3 traversal search path planning.
(a)(b)(c)(d)(e)(f)Figure 12
Obstacle space path planning. (a)Ptarget1=115,48,2 obstacle space path planning. (b) Ptarget2=115,48,38 obstacle space path planning. (c) Ptarget3=115,48,98 obstacle space path planning. (d) Ptarget4=115,78,88 obstacle space compartment path planning.
(a)(b)(c)(d)


### 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.


## 4.1. The Experiment Design

In this experiment, the starting point is set asSpoint=0,0,0, the growth step of RRT algorithm is set as S=5cm, and the expansion radius of the spherical obstacle and the tubular obstacle are set as rse=2cm and rpe=0.5cm, 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 pointPtarget1=115,48,2, the compartment entrance point Ptarget2=115,48,38, and the top target point Ptarget3=115,48,98, 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 Ptarget1 as the target point, Figures 10(a) and 10(b) show the schematic diagram of path planning results when Ptarget2 and Ptarget3 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 pointSpoint=0,0,0 as the aircraft fuel tank entrance, S=5cm 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 Ptarget1=10,10,30, Ptarget2=40,10,30, and Ptarget3=88,10,30 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 Section2.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 asPtarget4=115,78,88. The experimental results are shown in Figure 12(d), and the experimental process data are recorded in Table 4.Table 1
Barrier-free space path planning data.
Target pointBending angle (rad)Rotation angle (rad)Rising distance (cm)End pointError (cm)Time (s)(115, 48, 2){0.66, 1.24, 0.97, 1.71}{0.27, −0.15, 1.80, 0.27}182.34(114.61, 48.10, 1.01)1.0813.62(115, 48, 38){0.14, 1.45, 0.82, 0.66}{0.20, −0.13, 1.79, 0.75}153.80(115.19, 48.40, 37.88)0.4612.31(115, 48, 98){1.27, 0.14, 1.55, 0.85}{0.01, −1.31, 2.09, −1.89}185.16(115.15, 48.10, 98.41)0.4517.67The rising distance represents the rising distance between the base coordinate system of the Continuum Robot and the fuel tank coordinate system.Table 2
Traversal search and path fitting path planning data.
SignTarget pointBending angle (rad)Rotation angle (rad)Rising distance (cm)End pointError (cm)Time (s)A(10, 10, 30){0.58}{0.79}32.77(10, 10, 30)02.68B(10, 10, 30){0.57}{0.76}32.63(10.04, 9.45, 30)0.550.003A(40, 10, 30){0.08, 1.91}{0.32, 0.24}59.49(37.46, 9.39, 31.34)2.945.42B(40, 10, 30){0.50, 3.02}{0.00, 0.25}95.72(41.32, 8.25, 30.00)2.190.61A(88, 10, 30){0.13, 1.76, 0.77}{0.17, 0.05, 2.91}104.04(88.11, 10.03, 30.05)0.138.79B(88, 10, 30){0.19, 2.26, 0.57}{0.00, 0.06, 2.89}148.55(83.29, 9.42, 30.00)4.751218.54Table 3
Obstacle space path planning data.
Target pointBending angle (rad)Rotation angle (rad)Rising distance (cm)End pointError (cm)Time (s)(115, 48, 2){0.10, 1.72, 0.46, 1.27}{0.51, 0.15, 2.45, 0.85}154.35(115.03, 48.11, 1.81)0.2213.72(115, 48, 38){1.39, 0.50, 0.29}{0.07, 1.31, 1.38}150(115.19, 48.20, 37.98)0.2710.91(115, 48, 98){0.09, 1.45, 0.46, 1.78, 0.97}{0.40, −0.09, 0.95, 2.88, 0.82}154.56(115.03, 48.32, 98.16)0.3517.43(115, 78, 88){1.41, 0.10, 1.44, 1.37, 2.23}{0.07, −1.88, 1.29, 2.29, −2.67}250(114.87, 78.07, 87.96)0.1527.14Table 4
Obstacle space compartment path planning data.
Target pointBending angle (rad)Rotation angle (rad)Rising distance (cm)End pointError (cm)Time (s)(115, 78, 88){1.41, 0.10, 1.44, 1.37, 2.23}{0.07, −1.88, 1.29, 2.29, −2.67}250(114.87, 78.07, 87.96)0.1527.14Figure 9
Ptarget1=115,48,2 path planning process. (a) RRT discrete paths. (b) Fitting path. (c) The path that satisfies the endpoint condition. (d) The final path.
(a)(b)(c)(d)Figure 10
Barrier-free space path planning. (a)Ptarget2=115,48,38 barrier-free space path planning. (b) Ptarget3=115,48,98 barrier-free space path planning.
(a)(b)Figure 11
Traversal search and path fitting path planning comparison. (a)Ptarget1 path fitting path planning. (b) Ptarget2 path fitting path planning. (c) Ptarget3 path fitting path planning. (d) Ptarget1 traversal search path planning. (e) Ptarget2 traversal search path planning. (f) Ptarget3 traversal search path planning.
(a)(b)(c)(d)(e)(f)Figure 12
Obstacle space path planning. (a)Ptarget1=115,48,2 obstacle space path planning. (b) Ptarget2=115,48,38 obstacle space path planning. (c) Ptarget3=115,48,98 obstacle space path planning. (d) Ptarget4=115,78,88 obstacle space compartment path planning.
(a)(b)(c)(d)


## 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.



---

*Source: 8826749-2020-12-22.xml*