Motion Planning of a Snake-Like Robot Using an Optimized Harmonic Potential Field

Abstract Snake-like robots have gained popularity in the last three decades for their ability to utilize several gaits in order to navigate through different terrains. They are analogous in morphology to snakes, tentacles, and elephant trunks. We propose a novel method of navigating a snake-like robot based on the Harmonic Field with Optimized Boundary Conditions (HFOBC) and a boundary following algorithm. We apply the HFOBC navigation function using a number of fictitious charges equally spaced on each link. These charges actively follow the potential field towards the target. Futhermore, a generalized mathematical model for an n-link snake-like robot based on Lagrange formulation has also been proposed in this paper.


− → p i
A vector from the robot to the obstacle O i − → p β A vector from the robot to the target β U i ( − → p i ) A repelling potential field due to the obstacle i U β ( − → p β ) Attractive potential field due to target β ϕ( − → p ) Harmonic potential field value for vector − → p L Minimum distance from the robot to the closest obstacle R Safety distance − → n A unit vector perpendicular to the boundary in the direction of the robot − → τ A unit vector tangential to the boundary in the direction of the negative gradient of the potential field

Introduction
Since the first snake-like robot designed by Hirose in 1972 at the Tokyo Institute of Technology [1], research in snake robots has continued to be improved in terms of the technical design and mathematical modeling. More recent designs of snake-like robots include the JPL Serpentine Robot [2], the ACM-R5 and the Perambulator-II [3]. Research has been aimed at mathematical formulation of snake motion, both kinematically [4] and dynamically [5], using robotic snakes with wheels [3], crawler tracks [6] or only the snake body itself [7]. Many useful applications have been recently proposed for snake-like robots such as search-and-rescue in di cult environments [8], subsea applications, pipe inspection, service robots, and even fire-fighting [9].
In the following, we summarize some of the known literature on motion planning of snake-like robots and follow with a short summary of the advantages of our proposed navigation algorithm. Takanashi et al. [10], [11] proposed a motion planning algorithm compromised of two local sensor based techniques for snake robots. These are Full Shape Modification (FSM) and Partial Shape Modification (PSM). The FSM generates the general shape of the snake robot. The general shape is passed to the PSM which will fine-tune the general shape based on the local sensory information. However, this type of motion planners is limited because of its reactive nature and lack of a global planning stage. Henning et al. [12] proposed a Voronoidiagram-based roadmap to guide the motion of the snake robot. A major drawback with this type of purely graph theory based motion planners is that the quality of the path is highly dependent on the roadmap, which is typically coarse. Liu et al. [13] proposed a snake-robot motion planning technique based on the solution of the Serpenoid-curve-fitting optimization problem using a two-layer Genetic Algorithm (GA), yet this technique has some drawbacks. This approach requires an expensive computation in order to solve the two-layer GA. Moreover, the convergence is not guaranteed in real-time. Conkur [14] and Conkur and Gurbuz [15] proposed a snake-like robot navigation algorithm using a harmonic potential field. They proposed a path following technique based on "following the path" which means "keeping the ends of the links on the path." Unfortunately, this is not always a safe assumption particularly when navigating in a cluttered environment. Cheng et al. [16] proposed the so-called Accompanied Snake (A-Snake). The A-Snake is a navigation technique that is applied to mobile robots in general, as well as snake robots. This method applies a rolling window optimization in order to generate elastic paths for robot navigation. It mainly depends on a large-scale camera network distributed on top of the environment. Although this approach can produce fairly good results, it is rather impractical since it requires several cameras mounted on the top of the environment to monitor the gait of the robot throughout its motion. Yagnik et al. [17] proposed using an artificial potential field method with Simulated Annealing (SA) for snake robot navigation in a tail-follow-head fashion. In their algorithm, they used a Gaussian-like potential field function which su ers from the well-known local minima problem. Although they used SA to escape local minima, we argue that this type of heuristic algorithms is not guaranteed to work for complex environment and more links (more than three). In this paper, we propose an optimized harmonic potential field which is proven to be local-minima free. While the current literature indicates that it is better to consider the whole body of the snake-robot for motion planning, we have added well-distributed points/charges throughout the body of the robot to account for the motion of each part. Clearly, this addition will result in more accurate and smoother motion planning than tail-follow-head motion planning. Furthermore, the boundary following algorithm will also improve the quality of the motion planning particularly when navigating through narrow corridors, as explained later. Moreover, we present a general n-link navigation framework and show through simulation that our algorithm works well for 3-link and 10-link mobile robots navigating in a cluttered environment. This paper is organized as follows: We start by describing our proposed mechanical design of the snake robot. The dynamics of the snake robot are described in Section 3. Since the dynamics of a high DOF snake robot are fairly complicated, we first describe the dynamics of a threelink mobile robot and then we proceed to the dynamics of n-link robot. In Section 4, we solve the inverse dynamics problem in the presence of the harmonic field with an optimized boundary conditions. A performance evaluation is described in Section 5. Finally, in Sections 6 and 7, we provide discussion and concluding remarks regarding this work.

Mechanical Design of a Snake-Like Robot
Our design for a snake-like robot consists of n almost similar links connected to each other by n − 1 revolute joints. The first two links are equipped with two motorized wheels where each wheel can move in any direction using a separate steering motor supported by an omnidirectional wheel. The rest of the links are mounted on free wheels/casters and equipped with a snake-body bending mechanism at the inter-link joints. The mechanism is illustrated in Figures 1 and 2.

Dynamics of the Snake-Like Robot
Without loss of generality, we start by illustrating the case of a 3-link mobile robot, then we generalize it to an n-link snake-like robot. Figure 3 shows the top view of a 3-link mobile robot having a number of fictitious charges on it. Since this robotic system has 5 DOF, we have chosen the following five independent variables as generalized coordinates: x 0 , y 0 , θ 1 , θ 2 , and θ 3 , where x i , y i are the coordinates of the center of gravity of the link i, θ i is the angle between the link i and the global x-axis, and Z i is the position of the beginning of link i as shown in Figure 3. From Figure 3, we can write:

Dynamics of the 3-Link Mobile Robot
(1) y 2 = y 0 + l 1 sin θ 1 + 0.5l 2 sin θ 2 , x 3 = x 0 + l 1 cos θ 1 + l 2 cos θ 2 + 0.5l 3 cos θ 3 , y 3 = y 0 + l 1 sin θ 1 + l 2 sin θ 2 + 0.5l 3 sin θ 3 . We use Lagrange's Equations to describe the motion of the 3-link mobile robot: where q j is the j th generalized coordinates, and Q q j is the corresponding generalized force. The expression for the kinetic energy can be written as follows: For simplicity, we denote x 0 and y 0 as x and y. Also, we assume that all links are geometrically identical and therefore, we write l 1,2,3 = l, and m 1,2,3 = m, and the moment of inertia of each link rotating around its center of gravity is I 1,2,3 = I = 1 12 l 2 m. Substituting Equation (1) into (3), we obtain the following expression in terms of the generalized coordinates: In order to prevent the links from folding onto each other excessively, a fictitious torsional spring is added between each two consequent links. The equation describing the potential energy of these springs is: where k is the spring constant.
Substituting Equations (4) and (5) into Equation (2) and solving, we obtain the following equations of motion [18], where M is the inertia matrix: C (q,q)q represents the Coriolis and centrifugal terms: G(q) represents the forces derived from the potential energy: and Q ∈ R 5 is a vector of generalized forces to be computed later.

Dynamics of the n-Link Snake-Like Robot
In the following, we propose a general formula that can be applied to any n-link snake-like robot to compute the components of the equations of motion in Equation (6). We apply the same methodology adopted in the previous section, to obtain the components of the classical equation of motion.

Inertia Matrix
The generalized inertia matrix M ∈ R (2+n)×(2+n) and can be divided into three di erent matrices M 11 , M 12 , M 21 and M 22 : The matrix M 11 ∈ R 2×2 is a diagonal matrix where The matrix M 12 = M 21 T ∈ R 2×n can be written as: The matrix M 22 ∈ R n×n can be written as:

Coriolis and Centrifugal Terms Vector
The generalized C (q,q) ∈ R n matrix can be written as: where C 1 is 2 × n matrix defined as, and C 2 is a n × n matrix defined as,

Forces due to the Potential Energy G(q)
The generalized potential force vector is denoted G(q) ∈ R n . Since we assume that the motion occurs in a 2D plane parallel to the ground, the only potential energy forces are the damping torsional spring forces between each link. Thus, G(q) and can be written as: where, Here j is the index of each link.

Generalized Forces due to Actuator Forces
After finding M, C and G from Equation 6, we proceed to find the generalized forces Q a , due to the actuators. The j th joint is actuated using a bending torque − → T j applied between each two links. The traction comes from two omni-directional wheels equipped with traction motors at the two ends of the first link. The two traction forces are denoted − → p 1 and − → p 2 , respectively. For an n-link snake robot there are n + 2 generalized forces associated with n + 2 generalized coordinates and n + 3 actuator forces. Since the number of actuator forces is more than the number of generalized forces, this robot is an overactuated system.
Let us consider the case of the 3-link robot again. The generalized forces can be found using the virtual work principle [19]. The k th generalized force due to the actuators can be found by computing the work δw k produced by actuator forces when a small virtual displacement δq k is enforced on the system while all other q j are held constant, First, we consider the virtual displacement δ − → x = δx − → i . Clearly, the only actuator forces which will contribute to the corresponding virtual work δw x will be − → p 1 and − → p 2 , see Figure 4.
In the general case of n-link snake robot, one can show that:

Control Forces Design
In the following, we present our navigation algorithm for the snake robot using an optimized harmonic potential field.

Harmonic Potential Field with Optimized Boundary Conditions
The harmonic potential field [20] is given by the solution ϕ(r) : R 2 → R of Laplace's Equation: ∇ 2 ϕ = 0, with the boundary conditions: where ∂β is the union of the boundaries of all obstacles, ∂T is the target boundary, and c 1 and c 2 are constants.
The force applied to the robot is then designed to be the negated gradient of the potential field, i.e.

− →
It has been reported in [21] and [22] that the harmonic field with uniform boundary conditions, Eq. 22, does not have a smooth variation outside the target region. In [23], we proposed the Harmonic Field with Optimized Boundary Conditions (HFOBC) combined with Boundary Following Algorithm (BFA) [24]. The HFOBC abandons the uniformity of boundary conditions used in the regular harmonic field, Eq. (22), and instead applies ϕ(r) = ψ(r) whenever r ∈ ∂β, where ψ(r) is the length of the shortest path from r to the target boundary. By definition ψ(r) = 0 on the target boundary. This results in a harmonic field with optimized boundary conditions, which can be written as: ϕ(r) = ψ(r) for r ∈ ∂β ∪ ∂T .
The boundary conditions are optimized in the sense that ψ(r) represents the minimum distance to the target. These distances can be approximated using Dijkstra's algorithm on a finite-element mesh or other spatial discretization of the configuration space. Solving the problem in Eq. (23) results in the harmonic potential field whose contours are shown in Fig. 6. Clearly, the resulting field is free of local minima.

Motion Planning
To apply the HFOBC, the robot boundary is assumed to hold a set of fixed discrete charges interacting with the already developed electrostatic potential field. The physical analogy is not pursued accurately, since the moving robot charges will create a magnetic field that will complicate the physics. These magnetic e ects are assumed to be negligible in the adopted analogy. This modification can be made by assuming that a number of fictitious charges are fixed on the robot, as can be seen in Figure 7. Since charges tend to follow the streamlines, it is desirable to assign one of the charges to be the leader charge by simply increasing its value. The motion planning algorithm for a snake-like robot through a cluttered environment can be summarized as follows: 1. Apply adaptive allocation of charges.
2. Compute all potential field forces which take the robot toward the target using the HFOBC and the BFA.  3. Compute the actuator's forces in terms of the potential field forces.
4. Scale the actuators' forces to accommodate the maximum allowable forces and/or torques that can be applied on the actuators.

Adaptive allocation of charges
The distribution of the charges on the snake-like robot is important for proper navigation of the robot. If the number of charges on the segment is insu cient, collision may result between a sharp obstacle corner and a long uncharged piece of the robot. This can be avoided by choosing d c , the distance between two consecutive charges, such that: where d s is the safety distance, as can be seen in Fig. 8. Note that since the safety distance is adaptive, the charge separation can be adaptive as well.

Generalized Forces for n-Link Snake Robot due to Potential Field Forces
In the following, we find the generalized forces due to the forces acting on the charges shown in Figure 9. We start by the simple example of the 3-link mobile robot. We denote the generalized forces as: Let us assume that there are 3 charges whose forces are denoted − → where, − → r i k represents the position of the charge k on the link i, and δ − → r i k represents the virtual displacement of charge − → r i k .
To find the generalized force Q c x , we apply a virtual increment δ − → x = δx − → i = δ − → r i k to the first charge (x 0 , y 0 ), as illustrated in Figure 10.
Then, every charge's force contributes a virtual work proportional to its x-component: and in short, (26) Figure 9. The potential field forces applied to the three-link snake robot. Similarly, we find the generalized force Q c y as: Next, we proceed by computing the generalized forces corresponding to virtual angular displacement, Q c θ 1 , Q c θ 2 , and Q c θ 3 . Note the two examples of adding the virtual displacement δθ 1 in Figure 11 and the virtual displacement δθ 2 in Figure 12. First, a general expression for the vector − → r j k is derived: where − → Z j is the end-point vector (shown in Figure 9), which can be written as:  Next, we proceed by computing δ − → r i k in terms of δθ j . Obviously, From Equation (28) and (29), and after applying straightforward algebra, we find a general expression for as follows: Substituting Equation (30) into Equation (25), we find the generalized force Q θ 1 as: Similarly, we find expressions for Q θ 2 and Q θ 3 as follows: Now, we can find the generalized forces for the n-link mobile robot where each link j has N j charges acting on it. We let the generalized forces vector be denoted as We apply the same approach of finding the generalized forces of the 3-link robot to the n-link robot, and we obtain the following: and for j = 1, ..., n − 1, The last link is a special case, Note that the number of charges on the last link cannot be equal to 1.

Computing the Actuators' Forces in terms of Potential Field Forces
In this section, we compute the forces of the actuators in terms of the forces of the charges. To find actuators' forces, we equate the generalized forces corresponding to the actuator forces and the generalized forces corresponding to the charges' forces. For the 3-link robot, we obtain the following set of five equations, with the six unknowns p 1x , p 1y , p 2x , p 2y , T 1 and T 2 . This is an overactuated system, and there are infinitely many solutions.
Rearranging Equation (39), we find expressions for the torques T 1 and T 2 as follows: Therefore, we can write Equation (39) as follows: We will seek a least square solution, which minimizes the amount of traction as measured by − → p 1 2 + − → p 2 2 . In short, where, are all assumed to be known at a given configuration. The augmented cost is (in matrix notation): ] T ∈ R 2 and µ are the Lagrange multipliers. The solution of this minimization problem must satisfy: This solution is suboptimal in the sense that it minimizes the e orts

Scaling of Forces
Due to the high repulsive nature of the boundary following algorithm, we scale down all forces/torques with the same scaling factor, so that all forces/torques can be delivered. Assume that the maximum deliverable torque is T max and the maximum deliverable driving force/traction force for the first and the second link is p max . Hence, , and i = 1...n.

Performance Evaluation
We have simulated our algorithm on two types of robots. A three-link mobile robot and a ten-link snake-like robot.

Path Planning of 3-Link Mobile Robot
Here, we show the motion planning results of a three-link mobile robot. Each link is 5 m in length. Clearly, the robot is able to navigate though all the obstacles until it reaches the target. The robot maintains a safe distance to all boundaries, as can be seen from Figure 13.
As an example, we consider the speed profile for the first node in the robot. Figure 14 shows a practical speed profile. Note that the desired speed for the robot navigating without having any interaction with other boundaries is 5 m/sec. This value drops when the robot is interacting with a wall or a corner or with multi-wall-corner boundaries. This can be seen in Figure 14.

Path Planning for 10-Link Snake-Like Mobile Robot
Here, we show the simulation results for a 10-link snake-like robot. Figure 15 shows excellent navigation of the snake robot in the obstructed environment. This means excellent obstacle avoidance, no stagnation in local minima, no vibration or oscillation in the presence of obstacles, and practical speed profile. The safety distances to all boundaries are exactly as expected in the boundary following algorithm. Each link has three charges on it. Figure 16 shows the speed profile for the first charge. Clearly, the robot slows down when turning a corner or passing between a corner and a wall.

Discussion and Future Work
In this section, we discuss the main advantages of the proposed work and present several future plans for further development.
In this research, the gait of the robot is computed using an optimized  harmonic artificial potential field. This potential field is free of local minima, which makes it appealing for robust obstacle avoidance. Furthermore, the introduction of the well-distributed charges makes the planned motion very smooth. This approach introduces improvement to the tail-follow-head approach adopted in the literature [17], where the potential field is computed only for the head. Moreover, we present a novel presentation of the inverse dynamics of the snake-like robot derived from the potential field forces. As future work, we plan to extend this work to 3D environments. Also, we plan to implement this work on a wirelessly controlled snake-robot equipped with laser and vision sensors.

Conclusions
We have developed a novel method for the navigation of a snake robot. The navigation method is based on the optimized harmonic field and boundary following algorithm. Fictitious charges well-distributed on each link of the snake robot have been used to navigate the robot us-ing the HFOBC. This produces smooth motion and excellent obstacle avoidance. A generalized mathematical model for an n-link snake-like robot based on Lagrange formulation has been developed. The inverse dynamics problem for the proposed mechanism of a snake-like robot has been resolved. The simulation results show excellent navigation in the presence of obstacles and the speed profiles seem practical and correspond with the desired speed limit around the obstacles.