Trajectory generation for a walking in-pipe robot moving through spatially curved pipes

In this paper, a walking in-pipe robot is studied. The robot has six legs, each consisting of 3 links connected via rotary joints. The legs are attached to the robot’s body. The work is focused on the problem of generating desired position and orientation for the robot’s body, using a given footstep sequence. An iterative geometric algorithm for generating orientation sequence is proposed. The problem of finding the desired position of the center of mass of the robot’s body is formulated as a problem of minimizing stretching of the robot’s legs during steps. Also, an analytical solution for inverse kinematics problem has been given. All proposed algorithms do not require extensive calculation and use basic algebraic operations.


Introduction
The main use of in-pipe robots is the transport of specialized equipment, which, in its turn, is used for diagnostics and maintenance of the pipeline [1,2]. Having the equipment inside the pipeline is sometimes the only option other than excavation of the pipe, which is associated with significant costs. Additionally, being able to access the inner surface of the pipe can provide for a better detection of defects that occur there. It also provides a direct way to monitor the deposits on the inner surface of the pipe. Sensor systems employed in such inspection tasks are discussed in paper [3]. The use of in-pipe robots for cleaning the pipelines was also proposed in [4].
There are several types of in-pipe robots. Many of the proposed classifications distinguish seven or more different types [5,6]. Paper [7] is devoted to the study of motion of worm-like robot; the work [8] describes the parallel-link robot with flexible structure. Papers [9,10] present the results of numerical simulations in-pipe robot based on the vibration principle of movement. All of the above robots are used for inspection of pipelines. In this paper we focus on walking in-pipe robots. This type of robots is particularly interesting because of its ability to "step over" obstacles, potentially allowing such robots to navigate pipelines with branches, matter deposits and other types of obstacles.
Some of the walking robot designs are shown in papers [11][12][13][14][15][16][17]. In works [11][12][13] an eight legged in-pipe robot and its sensory system are studied. In paper [14] a six legged in-pipe robot designed for motion in spatially curved pipes is discussed. Articles [15][16][17] focus on planar in-pipe robots with four legs, considering designs with a solid and segmented main link.
We should note that there are a number of problems associated with controlling walking in-pipe robots. That includes step sequence generation, trajectory generation, inverse kinematics, controller design that takes into account information about contact with the inner surface of the pipe, observer design and others. This is similar to the set of problems associated with other types of walking robots. Many of the solutions for these problems found for different types of walking robots are viable for in-pipe walkers as well; this is discussed in paper [14] in more detail.
Some of the listed above problems have been studied specifically for the case of in-pipe walking robots. In papers [14,15] step sequence generation algorithms are proposed for planar and spatial in-pipe walking robots. In work [15] an analytic solution for inverse kinematics problem is given for a 9 link planar robot. In [16] a quadratic programming-based controller design for an in-pipe walking robot is proposed, able to take into account actuator torque limits and mechanical constraints, preventing loss of contact and slipping of the leg. Additionally, inverse kinematics algorithms, control designs and state-estimator designs were studied in [18] for a bipedal walking robot, using approaches that can be translated to in-pipe robots.
In this paper we focus on trajectory generation problem for the robot's body (the main link that all robot's legs are connected to). Knowing position and orientation of robot's body and robot's footstep sequence allows us to solve inverse kinematics problem for each leg individually, obtaining all joint angles. If there are three joints in each leg and the end effector of the leg (its foot) has three translational degrees of freedom when the robot's body is fixed, then in this case the inverse kinematics problem has a finite number of solutions that may be found analytically. For such a case, it should be possible to write an efficient inverse kinematics algorithm that can be executed using on-board computer without producing excessive computational load. Also, knowing robot's body position and orientation allows us to estimate the feasibility of the robot's trajectory with respect to the geometry of the pipeline. This paper can be considered a follow-up on the work [15], where the problem of body position and orientation generation was solved for a planar robot. The goal of the paper is to derive a systematic approach for generating center of mass trajectory for the robot's main link, as well as time functions defining its orientation.

Robot description
In this paper, we consider a six legged in-pipe robot with 19 individual links. Each leg includes three links connected via rotary joints and is attached to the robot's body. The construction of the robot is shown in Figure 1. . We denote the position of these points as C r , Fi r , Ei r , Di r and Ki r accordingly. The orientation of the body is defined by a directional cosines matrix T .
If C r and T are given, then we can determine Fi r using the following expression: where the superscript denotes the reference frame, ) 0 ( r means the vector is expressed in world frame Oxyz and ) 1 ( r means it is exressed in local frame Cx 1 y 1 z 1 .
Furthermore, we assume that links F i E i have such shape, that rotations in joints F i don't change the positions of points E i .

Determining desired position and orientation of the robot's body
The algorithm described in this section discretizes robot's trajectory and aims to find T and C r for every discrete point in time j t . We assume that we are given is described in detail in [14].
The algorithms includes two steps. First, we find the desired orientation of the robot's body, and then the position of its center of mass.

Determining desired orientation
First, we define directional vector K σ for the footsteps Ki r as follows: The directional cosines matrix ) ( j t T is then given by the following expression: where is the matrix representing rotation around axis FK e by angle FK α [19]. We should note that this method can accumulate error associated with roll angle (in roll-pitch-yaw representation of rotations). This can be avoided by performing an additional rotation, using K σ as an axis.

Determining desired position
To find desired position of the center of mass ) ( j C t r we find the vector p that shifts the previous its position ) to the new one, such that the resulting new positions of points F i minimize the distance between them and points K i . This condition aims at minimizing stretching of the robot's legs, allowing them to stay further away from kinematic singularities.
Then the vector p given by the following expression: This can be formulated as a quadratic program or can be solved directly using least squares method. To do the latter, we define a difference vector d and matrix C : where I is a 3 by 3 identity matrix, and both C and d have 18 rows. Then the solution for least squares can be found as: Thus, the new position of the center of mass is given as:

Generating continuous trajectories
In order to generate continuous trajectories from the obtained sequence of positions and orientations, we can use interpolation. Sequence ) ( j C t r can be directly interpolated using splines. In order to interpolate orientations, they need to be converted to a different representation, because interpolating sequence of directional cosines matrix can produce matrices that would not be orthonormal and thus would require orthogonalization. Instead, we can replace matrices ) ( j t T with quaternion ) ( j t q describing equivalent rotations [19]. This sequence of quaternions can then be interpolated using special algorithms, such as Slerp [20]. This interpolation can be used to recover rotation matrices T for any moment in time, or directly, by modifying formula (1) and other kinematics expressions accordingly. Figures 2 and 3 shows a sequence of positions of robot's body inside a pipeline (legs are not drawn, pipeline is opaque in Figure 2 and transparent in Figure  3). Figure 2 shows that the proposed method allows generating a sequence of positions of the center of mass of the robot. The robot always stays roughly at the center of the pipe, and keeps its orientation "parallel to the pipeline's direction".  r with the next formula:

Inverse kinematics algorithm
where FiEi r is vector drawn from point F i to E i . Figure 4 shows the leg of the robot. In the Figure 4, l 1 and l 2 and lengths of the links E i D i and D i K i and ψ i , φ i,1 and φ i,2 are joint angles.   Let us consider vector EiKi r drawn between points E i and K i . Using formula (11) Then we rotate the vector EiKi r so it would lie parallel to Ozx plane : With this we can consider a simple planar inverse kinematics problem, illustrated in Figure 5. The solution for the planar inverse kinematics problem is given by the next expressions: Using these expressions we can solve for ψ i , φ i,1 and φ i,2 triplets for each leg, thus solving inverse kinematics problem for the robot. Figure 6 shows the result of applying these algorithms, demonstrating obtained orientations of robot's legs while it moves through a curved pipe. We can see that the robot bends its "knees" in the same direction, which is the result of the chosen approach. This can be controlled by changing signs in the expressions (14) and (15). Similar problem was discussed in [15,21].
We should note that this algorithm does not handle singularities, and it would fail in the case when a point can't be reached. There are easy ways to check if the algorithm will fail, for example by using the following condition: If the leg segments are not of equal length, we can add another condition, requiring that the length of the longer segment would be less or equal to the length of the other segment plus i L . If the conditions are not satisfied, then the inverse kinematics problem for the leg has no solution.

Conclusions
In this paper a six legged in-pipe walking robot was studied. The aim of the study was to provide an efficient algorithm for generating positions and orientations of the robot's body, and to show how it can be used to derive an analytic solution for inverse kinematics problem.
The proposed method for generating center of mass trajectory is based on interpolation of the sequence of positions of the center of mass. That sequence is in its turn found by considering the problem of minimizing the distance between the footsteps locations (points of contact where the robot needs to step to) and the bases of robot's legs. This problem can be solved algebraically for the case when robot's orientation is fixed.
The orientation is found by a heuristic-based approach, which uses so-called directional vectors, which indicate where the robot should be orientated. The procedure of generating a sequence of orientations is iterative and requires initialization, although the initial orientation supplied to the algorithm does not need to be very accurate. Then the sequence of orientations can be represented as a sequence of quaternions and interpolated.