Step Climbing Control of Snake Robot with Prismatic Joints

Abstract

The ultimate goal of this research study is to perform continuous rather than sequential movements of prismatic joints for effective motion of a snake robot with prismatic joints in a complex terrain. We present herein a control method for robotic step climbing. This method is composed of two parts: the first involves the shift reference generator that generates the joint motion for climbing a step, and the other is use of the trajectory tracking controller, which generates the joint motion for the head to track the target trajectory. In this method, prismatic joints are divided into those that are directly controlled for climbing a step and those that are represented as redundancies. By directly controlling the link length, it is possible to prevent the trailing part from back motion when climbing a step, and to avoid a singular configuration in the parts represented as redundancies. A snake robot that has rotational and prismatic joints and can move in three-dimensions was developed, and the effectiveness of the proposed method was demonstrated by experiments using this robot. In the experiment, it was confirmed that the proposed method realizes the step climbing, and the link length limitation using the sigmoid function works effectively.

1. Introduction

Despite their long, slender, and simple shape, biological snakes are able to move across flat surfaces and walls, and climb up and down trees. In addition, some snakes swim underwater and glide through the air. To move in different environments, snakes use different modes of locomotion. Snakes use four main land movements, namely, serpentine, sidewinding, rectilinear, and concertina [1]. Many robots have been developed to mimic the various locomotion patterns of snakes [2].

Serpentine movement is the typical locomotion of biological snakes. The snake uses the difference in frictional force between the direction of its trunk and the direction perpendicular to it to generate a propulsive force through the lateral undulation of its body. For the purpose of performing biological-like motion, a serpenoid curve [1], three-dimensional motion [2,3], central pattern generator (CPG)-based control [4,5], and decentralized control [6] have been proposed. In contrast, numerous studies have been conducted using a model-based control approach in which the controller was designed based on a mathematical model. This approach enables the autonomous generation of snake robot behaviors to achieve control objectives, and the trajectory tracking of the robot head is realized on a flat plane [7,8,9], on a step [10], and on two nonparallel planes [11]. Accordingly, control methods have been proposed for path tracking of the center of mass [12] by using a model that considers the slipping of wheels. These studies dealt only with joint rotations and did not consider link extension or shrinking at all. In the cases in which cord-like robots were controlled with extendable mechanisms, predesigned peristalsis is mainly used. Robots that mimic the peristalsis performed by earthworms [13] have been developed and researched [14,15,16,17,18]. In addition, a robot that mimics the behavior of an inchworm has been studied in [19,20]. In contrast, a predesign method that was not a biomimicry was proposed in [21]. In this method, joint rotation and link extension/shrinking were predesigned as continuous curves, and three-dimensional motion with simultaneous joint rotation and link extension/shrinking were realized.

However, when using sequence control or a predesigned method described above, only fixed extendable movements can be performed. For example, locomotion that is designed to execute along a straight path cannot be applied directly to a turning motion. Thus, an extension and shrinking motion must be designed for each assumed motion. Therefore, when the environment or motion changes, it is desirable to change adaptively the extension/shrinking of the robot’s link and the rotation of the robot’s joints according to the situation.

Additionally, a head trajectory tracking control method for a two-dimensional plane was proposed in [22] as a method to utilize adaptive link extension/shrinking and joint rotation. In this method, the degrees of freedom for link stretching and joint rotation are represented collectively as redundancies, thereby allowing the robot to adaptively stretch links and rotate joints to achieve secondary control objectives other than the head trajectory tracking. In addition, as the link length limit for stretching is taken into account, the link lengths do not exceed hardware limits. However, verification has only been conducted based on simulations, and no experiments on actual equipment have been conducted.

The objectives of this study are to extend the control method proposed in [22] to nonplanar environments, and to utilize adaptive extension and shrinking of the links in more complex environments for a snake robot with extendable links. As the first step, we assume a step composed of two parallel planes as an environment, and propose a control method for a snake robot to move in the environment. The snake robot to be controlled consists of rotational joints, prismatic joints, and passive wheels; and the lengths of its links can be varied by the prismatic joints. The robot is described as a redundant system with redundancy with respect to link extension/shrinking and joint rotation. Furthermore, redundancy can be used to achieve secondary control objectives. As a way to keep the link length within hardware limits, a sigmoidal function was introduced into the model, as described in [22]. The effectiveness of the control method was demonstrated by experiments using an actual robot.

To ascend or descend a step, the robot must move between two planes with proper rotation of joints and a proper link length. If no consideration is given, the robot’s wheels will not be in proper contact with the respective planes, and the robot will not be able to generate the force from its wheels to move forward. To maintain proper contact between the wheels and planes, the link lengths should be expressed as controlled variables and controlled directly. However, this is inconsistent with the expression and use of the extension and shrinking degrees of freedom as redundancy. Therefore, in this study, the link length is divided into two parts: one for direct control and the other for use as redundancy. The two parts are used together to solve the problem.

The contributions of this study are listed below.

• We propose a control method for climbing a step by extending and retracting a link, and by rotating a joint By dividing the degrees of freedom of the link lengths into two parts, one part controls the link length directly, and the other part uses the link length as a redundancy. The proposed control method can achieve both of the following: (i) link extension/shrinking to move between two planes to achieve step climbing, and (ii) link extension/shrinking in accordance with joint rotation.
• We have developed an actual snake robot that can extend and retract links by prismatic and rotate joints, and demonstrate the effectiveness of a control method that adaptively utilizes the extension and shrinking of degrees of freedom through experiments. This study was the first demonstration using an actual robot, as [22] only verified the method with simulations.

2. Model

2.1. Problem Setting

Figure 1 shows a snake robot which can rotate its joints and change its link length and the step environment. The robot consists of variable-length links connected alternately by yaw and pitch rotational joints. It also comprises a pair of passive wheels mounted on the same axis as the pitch joints. The link changes the length according to the motion of the prismatic joint. Passive wheels simulate friction conditions. This means that wheels are less likely to skid in a manner similar to biological snakes. Accordingly, the robot can move while undulating by rotating joints properly. The assumed environment is a step with a height equal to h. The plane on the step is called the front plane, and the plane that the robot initially touches is called the rear plane.

This robot can arbitrarily change the link length. In the implementation of the motion control of this robot, the use of the degrees of freedom about the link length (telescopic degrees of freedom) is an important issue. In this study, the telescopic degrees of freedom are divided into two parts for control: the part that is directly controlled for the shift of connecting part, and the part that is expressed as redundancy and used for singular posture avoidance.

In designing the step-climbing motion, we adopted the method of [10] for the snake robot without prismatic joints. This method introduces a connecting part in the body of the snake robot that connects the forward and rear planes. By shifting the connecting part backward at the appropriate time, the robot can move from the rear to the front plane. By setting the yaw joint angle included in the connecting part to zero, the wheels at the both end of the connecting part can be properly contacted with respect to both the forward and rear planes. The robot is represented by two models. One model is a projection onto the $xy$-plane, and the other is a pitch-angle model for the connecting part.

Figure 2 shows the projection model onto the $xy$-plane. In this model, let $l_{fi}$ and $l_{bi}$ be the forward and backward link lengths of the ith pair of passive wheels, n be the number of pairs of passive wheels, $\mathit{w}={[x_h,y_h,{\theta }_h]}^{\top }$ be the position and orientation of the robot’s head, ${\varphi }_i$ be the ith yaw joint, ${\psi }_i$ be the relative angle of the ith pitch joint, and ${\psi }_h$ be the absolute pitch angle of the head link. Let us define $\mathit{\varphi }={[{\varphi }_1,\cdots ,{\varphi }_n]}^{\top }$, $\mathit{\psi }={[{\psi }_1,\cdots ,{\psi }_{n-1}]}^{\top }$, ${\mathit{l}}_f={[l_{f1},\cdots ,l_{fn}]}^{\top },{\mathit{l}}_b={[l_{b1},\cdots ,l_{bn-1}]}^{\top }$, ${\mathit{l}}_{all}={[l_f^{\top },l_b^{\top }]}^{\top }$, ${\Psi }_i={\psi }_h+{\sum }_{j=1}^{i-1}{\psi }_j$, $|{\Psi }_i|<\frac{\pi }{2}$.

2.2. Kinematic Model

In the kinematic model, it is assumed that the wheels do not skid in the transverse direction. The coefficient of friction in the transverse direction of the wheel is much larger than the coefficient of friction in the longitudinal direction (rolling friction), and the input is derived to satisfy the constraints (the input that prevents the wheel from skidding) based on the control model. Therefore, wheel skidding is negligibly small. In contrast, model errors due to joint angle resolution and mechanical stiffness of the robot can cause minor skidding. Although skidding causes an error of the controlled variables from the target, it can be corrected by the feedback control. Let the x and y positions of the center of ith pair of wheels be $x_i$ and $y_i$, respectively, and the absolute angle of the links adjacent to the ith pair of wheels be ${\theta }_i$. The velocity constraint equation, which ensures that the passive wheel will not skid, is expressed as

$$\begin{array}{c}\hfill \dot{x_i}sin{\theta }_i-\dot{y_i}cos{\theta }_i=0.\end{array}$$

(1)

A simultaneous set of Equation (1) for all pairs of passive wheels is represented as

$$\mathit{A}\dot{\mathit{w}}=\left[\begin{array}{cc}{\mathit{B}}_{\varphi }& {\mathit{B}}_l\end{array}\right]\mathit{u}$$

(2)

$$\begin{array}{cc}\hfill \mathit{u}& =\left[\begin{array}{c}\dot{\mathit{\varphi }}\\ {\dot{\mathit{l}}}_{all}\end{array}\right],\hfill \end{array}$$

(3)

where u is the control input, the ith row of (2) represents the velocity constraint equation for the ith wheel pair, and $\mathit{A}\in {\mathbb{R}}^{n\times 3},{\mathit{B}}_{\varphi }\in {\mathbb{R}}^{n\times n},{\mathit{B}}_l\in {\mathbb{R}}^{n\times (2n-1)}$. Let $B_{l[i:j]}$ be the matrix consisting of $i,\cdots ,j$ columns of ${\mathit{B}}_l$ and ${\mathit{B}}_{\varphi }$:

$${\mathit{B}}_{\varphi }=\left[\begin{array}{cccc}{l}_{f1}& & & \mathbf{0}\\ & l_{f2}& & & \\ & & \ddots & \\ \ast & & & l_{fn}\end{array}\right],$$

(4)

$$\begin{array}{cc}\hfill B_{l[1:n-1]}& =B_{l[n+1:2n-1]}\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill & =\left[\begin{array}{ccc}0& \cdots & 0\\ b_{2,1}& & \mathbf{0}\\ ⋮& \ddots & \\ b_{n,1}& \cdots & b_{n,n}\end{array}\right],\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill B_{l\left[n\right]}& =0,\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill b_{[i,j]}& =-sin\sum _{k=j+1}^i{\varphi }_k.\hfill \end{array}$$

(8)

The yaw angle $\mathit{\varphi }$ and the actual link length (not the link length of the projection model) of the robot are limited depending on hardware. To constrain the jth link length of the actual robot ${\overline{l}}_j$, $l_j$ is expressed using the standard sigmoidal function and the mediating variable ${\gamma }_j$. The standard sigmoidal function is expressed as

$$\begin{array}{c}\hfill f\left(x\right)={\displaystyle \frac{1}{1+exp(-x)}}.\end{array}$$

(9)

Using the sigmoidal function (9), the jth actual link length of the robot ${\overline{l}}_j$ can be expressed as

$$\begin{array}{cc}\hfill {\overline{l}}_j& =({\overline{l}}_U-{\overline{l}}_L)f\left({\gamma }_j\right)+{\overline{l}}_L,\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill {\dot{\overline{l}}}_j& =({\overline{l}}_U-{\overline{l}}_L)\{1-f\left({\gamma }_j\right)\}f\left({\gamma }_j\right),\hfill \end{array}$$

(11)

where ${\overline{l}}_U\ne 0$ and ${\overline{l}}_L\ne 0$ are the maximum and minimum values of the link length, respectively, and ${\overline{l}}_L\le {\overline{l}}_j\le {\overline{l}}_U$. The link lengths $l_{fi},l_{bi}$ of the projection model are calculated using the absolute pitch angle ${\Psi }_i$, along with the mediating variable ${\gamma }_{fi},{\gamma }_{bi}$, as

$$\begin{array}{cc}\hfill l_{fi}& =\{({\overline{l}}_U-{\overline{l}}_L)f\left({\gamma }_{fi}\right)+{\overline{l}}_L\}cos{\Psi }_i,\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill l_{bi}& =\{({\overline{l}}_U-{\overline{l}}_L)f\left({\gamma }_{bi}\right)+{\overline{l}}_L\}cos{\Psi }_{i+1}.\hfill \end{array}$$

(13)

When the link length is expressed by Equations (10)–(13), the actual link length is limited to ${\overline{l}}_L\le {\overline{l}}_j\le {\overline{l}}_U$. Therefore, the link length does not exceed the movable limit, regardless of the input. Assuming that ${\psi }_h={\dot{\psi }}_h=0$ is valid as the robot’s head continues to move on the same plane, when it moves between the two planes [10], the following equation can be used.

$${\dot{\mathit{l}}}_{all}=\left[\begin{array}{cc}{\mathit{C}}_1& {\mathit{C}}_2\end{array}\right]\left[\begin{array}{c}{\dot{\mathit{\gamma }}}_{all}\\ \dot{\mathit{\psi }}\end{array}\right],$$

(14)

where

$$\begin{array}{cc}\hfill {\mathit{C}}_1& =\left[\begin{array}{cccccc}{g}_1\left({\gamma }_{f1}\right)cos{\Psi }_1& & & & & \mathbf{0}\\ & \ddots & & & & \\ & & g_1\left({\gamma }_{fn}\right)cos{\Psi }_n& & & \\ & & & g_1\left({\gamma }_{b1}\right)cos{\Psi }_2& & \\ & & & & \ddots & \\ \mathbf{0}& & & & & g_1\left({\gamma }_{bn-1}\right)cos{\Psi }_n\end{array}\right],\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill g_1\left(x\right)& =({\overline{l}}_U-{\overline{l}}_L)\{1-f\left(x\right)\}f\left(x\right),\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill {\mathit{C}}_2& =\left[\begin{array}{ccc}0& \cdots & 0\\ -g_2\left({\gamma }_{f2}\right)sin{\Psi }_2& & \mathbf{0}\\ ⋮& \ddots & \\ -g_2\left({\gamma }_{fn}\right)sin{\Psi }_{n-1}& \cdots & -g_2\left({\gamma }_{fn}\right)sin{\Psi }_{n-1}\\ -g_2\left({\gamma }_{b1}\right)sin{\Psi }_2& & \mathbf{0}\\ ⋮& \ddots & \\ -g_2\left({\gamma }_{bn-1}\right)sin{\Psi }_{n-1}& \cdots & -g_2\left({\gamma }_{bn-1}\right)sin{\Psi }_{n-1}\end{array}\right],\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill g_2\left(x\right)& =({\overline{l}}_U-{\overline{l}}_L)f\left(x\right)+{\overline{l}}_L,\hfill \end{array}$$

(18)

and ${\mathit{\gamma }}_f={[{\gamma }_{f1},\cdots ,{\gamma }_{fn}]}^{\top }$, ${\mathit{\gamma }}_b={[{\gamma }_{b1},\cdots ,{\gamma }_{bn-1}]}^{\top }$, ${\mathit{\gamma }}_{all}={[{\mathit{\gamma }}_f^{\top },{\mathit{\gamma }}_b^{\top }]}^{\top }$, ${\mathit{C}}_1\in {\mathbb{R}}^{(2n-1)\times (2n-1)}$, ${\mathit{C}}_2\in {\mathbb{R}}^{(2n-1)\times (n-1)}$.

By substituting Equation (14) into Equation (2), we can obtain the following:

$$\begin{array}{cc}\hfill \mathit{A}\dot{\mathit{w}}& =\mathit{B}\overline{\mathit{u}},\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill \mathit{B}& =\left[\begin{array}{ccc}{\mathit{B}}_{\varphi }& {\mathit{B}}_l{\mathit{C}}_1& {\mathit{B}}_l{\mathit{C}}_2\end{array}\right],\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill \overline{\mathit{u}}& =\left[\begin{array}{c}\dot{\mathit{\varphi }}\\ {\dot{\mathit{\gamma }}}_{all}\\ \dot{\mathit{\psi }}\end{array}\right].\hfill \end{array}$$

(21)

The model (19) changes depending on the grounded/lifted status of each pair of wheels. When the ith wheel pair is not touching the ground, the ith row of (19) disappears. We allocated a unique index known as the mode to each model to represent the overall status of the wheel pair. When the mode is $\sigma$ and the $i_{1}st,\cdots ,i_{n_{\sigma }}$ wheel does not touch the ground, Equation (19) becomes

$$\begin{array}{cc}\hfill {\overline{\mathit{A}}}_{\sigma }\dot{\mathit{w}}& ={\overline{\mathit{B}}}_{\sigma }\overline{\mathit{u}},\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill & =\left[{\overline{\mathit{B}}}_{\varphi }{\overline{\mathit{B}}}_l{\mathit{C}}_1{\overline{\mathit{B}}}_l{\mathit{C}}_2\right]\overline{\mathit{u}},\hfill \end{array}$$

(23)

where ${\overline{\mathit{A}}}_{\sigma }\in {\mathbb{R}}^{(n-n_{\sigma })\times 3},{\overline{\mathit{B}}}_{\sigma }\in {\mathbb{R}}^{(n-n_{\sigma })\times (4n-2)}$, and ${\overline{\mathit{B}}}_{\varphi }$ and ${\overline{\mathit{B}}}_l$ are the matrices in which the $i_1,\cdots ,i_{n_{\sigma }}$th rows are eliminated from $\mathit{A}$ and $\mathit{B}$, respectively.

In the following subsection, we consider the shift. The body area immediately behind the connecting part, which becomes the next connecting part when the shift is completed, is called the "connecting preparation part." In this method, the yaw joint angle of the connecting parts must be zero for the wheels to properly attach on the front plane, and the yaw joint angle of the connecting part and the connecting preparation part must be zero when shifting [10]. Therefore, the wheels behind the connecting part are ungrounded, and the yaw joint of the connecting preparation part is directly controlled to perform the shifting of the connecting part. The system also aims to reduce the influences of the shifting motion on the other parts of the connection by using the extension and shrinking of the links during the shift.

In mode $\sigma$, let ${\tilde{\mathit{\varphi }}}_{\sigma }\in {\mathbb{R}}^{n_{\varphi \sigma }}$ be a vector of yaw joints that should be directly controlled in the connecting and connecting preparation parts, and ${\mathit{l}}_{\sigma }\in {\mathbb{R}}^{n_{l\sigma }}$ be the link length that should be directly controlled, and ${\tilde{\mathit{w}}}_{\sigma }={[{\mathit{w}}^{\top },{\mathit{l}}_{\sigma }^{\top },{\mathit{\psi }}^{\top },{\tilde{\mathit{\varphi }}}_{\sigma }^{\top }]}^{\top }$. The kinematic model is obtained as

$$\begin{array}{cc}\hfill {\tilde{\mathit{A}}}_{\sigma }{\dot{\tilde{\mathit{w}}}}_{\sigma }& ={\tilde{\mathit{B}}}_{\sigma }\overline{\mathit{u}},\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill {\tilde{\mathit{A}}}_{\sigma }& =\left[\begin{array}{cccc}{\overline{\mathit{A}}}_{\sigma }& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\mathit{I}}_{n_{l_{\sigma }}}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {\mathit{I}}_{n-1}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& {\mathit{I}}_{n_{{\varphi }_{\sigma }}}\end{array}\right],\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill {\tilde{\mathit{B}}}_{\sigma }& =\left[\begin{array}{ccc}{\overline{\mathit{B}}}_{\varphi }& \overline{{\mathit{B}}_l}{\mathit{C}}_1& \overline{{\mathit{B}}_l}{\mathit{C}}_2\\ \mathbf{0}& {\mathit{C}}_{1\sigma }& {\mathit{C}}_{2\sigma }\\ \mathbf{0}& \mathbf{0}& {\mathit{I}}_{n-1}\\ {\mathit{B}}_{{\varphi }_{\sigma }}^{\prime }& \mathbf{0}& \mathbf{0}\end{array}\right],\hfill \end{array}$$

(26)

where ${\mathit{B}}_{{\varphi }_{\sigma }}^{\prime }$ is a selection matrix whose components are zeros or ones, and satisfies ${\mathit{B}}_{{\varphi }_{\sigma }}^{\prime }\overline{\mathit{u}}={\dot{\tilde{\mathit{\varphi }}}}_{\sigma }$. ${\mathit{C}}_{1_{\sigma }},{\mathit{C}}_{2_{\sigma }}$ are matrices consisting only of the columns of ${\mathit{C}}_1,{\mathit{C}}_2$ corresponding to $l_{\sigma }$, and ${\mathit{C}}_{1_{\sigma }}\in {\mathbb{R}}^{n_{l\sigma }\times (2n-1)},{\mathit{C}}_{2_{\sigma }}\in {\mathbb{R}}^{n_{l\sigma }\times (n-1)}$. Let us define ${\mathit{C}}_{\sigma }=[{\mathit{C}}_{1\sigma },{\mathit{C}}_{2\sigma }]$; then, ${\mathit{C}}_{\sigma }\overline{\mathit{u}}={\dot{\mathit{l}}}_{\sigma }$ is satisfied.

From (4) and (19), $\mathit{B}$ is a row of full rank if $l_{fi}\ne 0$. Given that $l_{fi}>0$ in an actual robot, $\mathit{B}$ is always a row of full rank. Given that $|{\Psi }_i|<\pi /2$, $\left[{\mathit{C}}_1{\mathit{C}}_2\right]$ is a row of full rank, $\left[{\mathit{C}}_{1\sigma }{\mathit{C}}_{2\sigma }\right]$ is also a row of full rank. Additionally, for ${\overline{\mathit{B}}}_{\varphi }$ and ${\mathit{B}}_{{\varphi }_{\sigma }}^{\prime }$, ${[{\overline{\mathit{B}}}_{\varphi }^{\top },{\mathit{B}}_{{\varphi }_{\sigma }}^{\prime \top }]}^{\top }$ have the property of being full rank. For example, if $n_{\sigma }=1$ and only the ith wheel does not touch the ground, then

$$\left[\begin{array}{c}{\overline{\mathit{B}}}_{\varphi }\\ {\mathit{B}}_{{\varphi }_{\sigma }}^{\prime }\end{array}\right]=\left[\begin{array}{ccccccc}{l}_{f1}& 0& & & \cdots & & 0\\ & \ddots & & & & & ⋮\\ ⋮& & l_{fi-1}& 0& 0& \cdots & 0\\ & & \cdots & *& l_{fi+1}& & \mathbf{0}\\ & & & & & \ddots & \\ *& & & \cdots & & & l_{fn}\\ 0& \cdots & 0& 1& 0& \cdots & 0\end{array}\right].$$

(27)

If the wheel is not touching the ground, ${\overline{\mathit{B}}}_{\varphi }$ is obtained by removing the row corresponding to the ungrounded wheel from ${\mathit{B}}_{\varphi }$, but ${\mathit{B}}_{{\varphi }_{\sigma }}^{\prime }$ is linearly independent of ${\overline{\mathit{B}}}_{\varphi }$ as (27). Thus, (27) is always full rank.

This kinematic model is derived based on the velocity constraint of the wheel. Therefore, this model and the control method described below will not work for robots with isotropic friction characteristics (e.g., nonwheeled snake robots) or in environments with very low friction, because the error of the controlled variables increases owing to the error of the model.

3. Controller

The robot generates undulation motion by changing yaw joints and link lengths, and shifts the connecting part by changing pitch joints and link lengths. The control objectives are the tracking of the robot’s head to the target trajectory and moving between steps. These are the main tasks. In addition, the avoidance of singular configuration [23] is introduced as a secondary control objective (subtask). Figure 3 shows the controller configuration. The controller consists of a shift reference generator and a tracking controller. The shift reference generator determines the robot’s mode $\sigma$ from the current robot’s situation, and calculates the target pitch joint angle ${\mathit{\varphi }}_d$ and the target lengths of some links $l_{\sigma d}$ to realize step climbing. The trajectory tracking controller calculates the yaw joint rotational velocity $\dot{\mathit{\varphi }}$ and the pitch joint rotational velocity $\dot{\mathit{\psi }}$, and mediating variable ${\dot{\mathit{\gamma }}}_{all}$ to achieve both tracking of the robot’s head to the target trajectory ${\mathit{w}}_d$ and the subtask. The ${\dot{\mathit{\gamma }}}_{all}$ is converted to an actual link length using (10)–(14), and is sent to the robot.

3.1. Trajectory Tracking Controller

For the kinematic model (24), the input $\overline{\mathit{u}}$ is designed as

$$\begin{array}{cc}\hfill \overline{\mathit{u}}& ={\mathit{u}}_{T_{\sigma }}+{\mathit{u}}_{K_{\sigma }},\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill {\mathit{u}}_{T\sigma }& ={\tilde{\mathit{B}}}_{\sigma }^{W†}{\tilde{\mathit{A}}}_{\sigma }\{{\dot{\tilde{\mathit{w}}}}_{\sigma d}-{\mathit{K}}_{\sigma }({\tilde{\mathit{w}}}_{\sigma }-{\tilde{\mathit{w}}}_{\sigma d})\},\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill {\mathit{u}}_{K\sigma }& =(\mathit{I}-{\tilde{\mathit{B}}}_{\sigma }^{W†}{\tilde{\mathit{B}}}_{\sigma }){\mathit{K}}_V\mathit{\eta },\hfill \end{array}$$

(30)

where ${\tilde{\mathit{B}}}_{\sigma }^{W†}$ is the weighted pseudoinverse of ${\tilde{\mathit{B}}}_{\sigma }$[24], ${\mathit{u}}_{T\sigma }$ is the input related to trajectory tracking, ${\mathit{u}}_{K\sigma }$ is the input related to redundancy, ${\mathit{K}}_{\sigma }$ is the positive definite diagonal matrix for gain, ${\mathit{K}}_V$ is the diagonal matrix for gain on redundancy, and $\mathit{\eta }$ is an arbitrary vector. ${\mathit{u}}_{T\sigma }$ is the input for convergence of the control point to the target, and ${\mathit{u}}_{K\sigma }$ is the input related to the redundant component and does not interfere with ${\mathit{u}}_{T\sigma }$. Therefore, ${\mathit{u}}_{K\sigma }$ does not interfere with the convergence of the control point to the target. By properly designing the $\eta$, a secondary task can be accomplished simultaneously with the main task of trajectory tracking of the control point. The design of $\eta$ is described below. As ${\tilde{\mathit{B}}}_{\sigma }$ is always full-rank, as explained in the previous section, from (19) and (28), we obtain the following closed loop system:

$$\begin{array}{c}\hfill {\tilde{\mathit{A}}}_{\sigma }\{{\dot{\mathit{w}}}_{\sigma }-{\dot{\mathit{w}}}_{\sigma d}+{\mathit{K}}_{\sigma }({\mathit{w}}_{\sigma }-{\mathit{w}}_{\sigma d})\}=\mathbf{0}.\end{array}$$

(31)

If ${\tilde{\mathit{A}}}_{\sigma }$ is a column of full rank, then ${\mathit{w}}_{\sigma }$ converges to $w_{\sigma d}$ as $t\to \infty$. However, the convergence of the controlled variable to the target value is not guaranteed when the column rank of ${\tilde{\mathit{A}}}_{\sigma }$ is not full. This case is called a singular configuration of the robot. The singular configurations resulting in rank deficiency of ${\tilde{\mathit{A}}}_{\sigma }$ are those in which all grounded wheel axes are parallel, or all extended lines of the axes with grounded wheels intersect at a single point [23]. In this study, these singular configurations are avoided by appropriately designing the input component related to redundancy ${\mathit{u}}_{K\sigma }$.

The ${\mathit{u}}_{K\sigma }$ in (28) is an input related to kinematic redundancy, which can achieve a subtask without affecting the trajectory tracking of the controlled variable. Let $V\left(\mathit{q}\right)$ be the evaluation function and $\mathit{\eta }$ be set as follows.

$$\begin{array}{c}\hfill \mathit{\eta }={\left[{\displaystyle \frac{\partial V}{\partial {\phi }_1}},\cdots ,{\displaystyle \frac{\partial V}{\partial {\phi }_{4n-2}}}\right]}^{\top },\end{array}$$

(32)

where $\mathit{\phi }={[{\mathit{\varphi }}^{\top },{\mathit{\gamma }}_{all}^{\top },{\mathit{\psi }}^{\top }]}^{\top }\in {\mathbb{R}}^{(4n-2)\times 1}$ and ${\phi }_i$ is the ith element of $\mathit{\phi }$. The time derivative of the evaluation function $V\left(\mathit{q}\right)$ is derived as

$$\begin{array}{cc}\hfill {\displaystyle \frac{\mathrm{d}V}{\mathrm{d}t}}& ={\displaystyle \frac{\partial V}{\partial \mathit{w}}}\dot{\mathit{w}}+{\displaystyle \frac{\partial V}{\partial \mathit{\phi }}}\dot{\mathit{\phi }}\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill & ={\displaystyle \frac{\partial V}{\partial \mathit{w}}}\dot{\mathit{w}}+{\mathit{\eta }}^{\top }{\mathit{u}}_{T\sigma }+{\mathit{\eta }}^{\top }{\mathit{u}}_{K\sigma }\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill & ={\displaystyle \frac{\partial V}{\partial \mathit{w}}}\dot{\mathit{w}}+{\mathit{\eta }}^{\top }{\tilde{\mathit{B}}}_{\sigma }^{W†}{\tilde{\mathit{A}}}_{\sigma }\{{\dot{\tilde{\mathit{w}}}}_{\sigma d}-{\mathit{K}}_{\sigma }({\tilde{\mathit{w}}}_{\sigma }-{\tilde{\mathit{w}}}_{\sigma d})\}+{\mathit{\eta }}^{\top }(\mathit{I}-{\tilde{\mathit{B}}}_{\sigma }^{W†}{\tilde{\mathit{B}}}_{\sigma }){\mathit{K}}_V\mathit{\eta },\hfill \end{array}$$

(35)

where $(\mathit{I}-{\tilde{\mathit{B}}}_{\sigma }^{W†}{\tilde{\mathit{B}}}_{\sigma })\ge 0$. The $\mathit{\eta }$ can contribute to increasing or decreasing V depending on the sign of ${\mathit{K}}_V$. The evaluation function V is designed as

$$\begin{array}{cc}\hfill V& =a_s{V}_s\hfill \end{array}$$

(36)

$$\begin{array}{cc}\hfill V_s& ={\displaystyle \frac{1}{det{\tilde{\mathit{A}}}_{\sigma }^{\top }{\tilde{\mathit{A}}}_{\sigma }}},\hfill \end{array}$$

(37)

where $a_s$ is a positive constant representing the weights. $V_s$ is the evaluation function for singular configuration avoidance, which is ∞ when the configuration of the robot is singular. Singular configuration is avoided by decreasing V with negative values for each component of ${\mathit{K}}_V$.

3.2. Shift Reference Generator

The shift reference generator determines the robot’s mode $\sigma$ based on the relative relationship between the robot, step, and the shift situation. If the shift start condition is satisfied, the shift of the connecting part is started, and the target values of the pitch joint angle and link length near the connecting part are calculated. The calculated target pitch joint angle ${\mathit{\psi }}_d$ and the target link length to be directly controlled ${\mathit{l}}_{\sigma d}$ are passed to the tracking controller, together with the mode $\sigma$.

When shifting the connecting part, the motion is designed to make the relative distance between the two ends of the connecting part before and after the shift constant to minimize the effect of the shift on other parts of the trunk. The shift operation is shown in Figure 4. Note that all yaw joint angles were set to zero for simplicity for the figure, and the figure is viewed from the $xz$ plane. Given that the link length was fixed in [10], as shown in Figure 4a, there was a risk in that the backward movement would occur during the shift, which would increase the joint angle of the body on the rear plane and cause it to exceed the limit value. In contrast, as the robot used in this study had changeable link lengths, the shift was performed so that there was no backward movement, as shown in Figure 4b. The specific target values of joint angles are omitted because they can be obtained only from plain geometrical relations.

The timing for starting the shift was set in the same way as [10]. Specifically, the shift was started when the yaw joint angle of the connecting part was near the target value (zero) and when the wheel ahead of the connecting part touched the ground.

4. Experiments

The robot developed for the demonstration of the effectiveness of the proposed control method is shown in Figure 5. This robot has yaw joints, pitch joints, passive wheels, and prismatic joint units. The link lengths can be changed using the prismatic joint units. In these units, a feed screw is rotated by the actuator through a flat gear, the screw nut translates along the feed screw, and the link length changes. Independent of the link length, the yaw joint can operate in the range of $\pm 91$ deg and the pitch joint in the range of $\pm 110$ deg.

This robot was developed for this study and can change the link lengths without interfering the motion range of its rotational joints. In addition, a link length limitation method [22] based on a sigmoidal function was verified for the first time using an actual robot.

The parameters used for the experiments are listed in

Table 1. Parameters of experiments.

Parameter	Value
n: The number of pairs of wheel	8
l: Head link length	0.100 m
${\mathit{l}}_f\left(0\right)$: Initial value of ${\mathit{l}}_f$	${[0.150,0.112,0.161,0.150,0.150,0.150,0.150,0.150]}^{\top }$ m
${\mathit{l}}_b\left(0\right)$: Initial value of ${\mathit{l}}_b$	${[0.112,0.161,0.150,0.150,0.150,0.150,0.150]}^{\top }$ m
$\mathit{\varphi }\left(0\right)$: Initial value of $\mathit{\varphi }$	${[0,0,\pi /6,0,-\pi /6,\pi /6,0,-\pi /6]}^{\top }$ deg
$l_U$: Maximum link length	0.190 m
$l_L$: Minimum link length	0.145 m
$W_i$: ith diagonal element of $\mathit{W}$	1.0(if $i\le n$), $1.0\times {10}^5$(if $n+1\le i$)
Diagonal element of ${\mathit{K}}_{\sigma }$	The element corresponding to $y_h$, ${\mathit{l}}_{\sigma }$, others are 3, 2, 1, respectively.
Diagonal element of ${\mathit{K}}_V$	The element corresponding to $l_{fi}$, $l_{bi}$, others are $-0.001$, $-2$, $-1$, respectively.
$a_s$	$5.0\times {10}^{-3}$
h: Height of the connecting part	0.23 m
${\mathit{w}}_d$: Target trajectory of $\mathit{w}$	${[0.025t,0,\pi +\pi /5sin(2\pi t/10)]}^{\top }$

. The experiment was conducted with motion capture equipment to obtain information on the head position and orientation. The robot climbed a step with a height of 0.20 m. When the robot was tested with a step height of $h=0.20$ m as a parameter in the input calculation, deformation occurred in the telescopic link owing to the insufficient rigidity of the robot; correspondingly, the robot could not move forward due to contact between the robot and the step. Therefore, we performed the experiment with $h=0.23$ m in the input calculation by taking into account the deformation of the robot.

As a result of prioritizing miniaturization and weight reduction, the prismatic joint unit had low-mechanical rigidity and was deformed when external forces acted on it. When a force was applied from the side, including during deformation, the frictional force generated inside the prismatic joint unit increased. However, the output of the unit was sufficiently large such that the telescopic motion was not inhibited, and the link length followed the target. The propulsion speed of the robot was reduced to satisfy the assumption that the wheels did not skid. At a low-propulsion speed (and a low acceleration), the force which acted on the wheel did not exceed the maximum static friction force, and the velocity constraint could be satisfied. As a result, the control error was small, and the robot could follow the target trajectory. In contrast, fast motion (high acceleration) caused the force acting on the wheel to exceed the maximum static friction force and increase the control errors due to wheel slippage. Therefore, this study slowed down the robot’s movement speed.

Figure 6 shows the robot’s behavior during the experiment, and Figure 7 and Figure 8 show the results. The position and orientation of the head $\mathit{w}$ tracked the target trajectory. In Figure 8b,d, the red-dotted line represents the target during the shift operation. It was confirmed that ${\mathit{l}}_f$ and ${\mathit{l}}_b$ also followed the target during the shift operation. With the exception of the periods during the shift operation, the link length was adaptively changed by the redundancy so that ${\mathit{l}}_b$ became longer. This is an effect of singular configuration avoidance by the redundancy similar to that reported in [22], and contributes to the amplification of the oscillations at the head toward the tail. The link lengths ${\overline{\mathit{l}}}_f,{\overline{\mathit{l}}}_b$ were confirmed to be within the limit set by the sigmoidal function. The full rankness of ${\tilde{\mathit{A}}}_{\sigma }$ of the column depends on ${\overline{\mathit{A}}}_{\sigma }$. Thus, $det{\overline{\mathit{A}}}_{\sigma }^{\top }{\overline{\mathit{A}}}_{\sigma }$ was used for the evaluation of the singular configuration. If the robot is in a singular configuration, this value is set to zero. The determinant $det{\overline{\mathit{A}}}_{\sigma }^{\top }{\overline{\mathit{A}}}_{\sigma }$ did not converge to zero. This means that the singular configuration was avoided.

However, the error in the leading position configuration $\mathit{w}$ increased after $t=70$. This was caused when the bottom part of the body contacted a step. As shown in Figure 9, the prismatic joint was deformed by the load of the tail lifted in a cantilever position after the tail shift, and the bottom part of the body contacted the step and could not be moved over the step to the tail. The same reason was applied to justify why it was necessary to use a larger value $0.23$ m for the step height h set during control compared with the actual value $0.20$ m. To solve this problem, it is necessary to improve the rigidity of the prismatic joints so that deformation does not occur. This is one of the subjects for future work.

5. Conclusions

We proposed a control method to realize step climbing for a snake-like robot with prismatic joints. The controller consisted of two parts: one was the shift reference generator which generated the joint movements for climbing the step. The other was the trajectory tracking controller which generated joint movements to track the head along the target trajectory. In this method, the prismatic joints were divided into those that were directly controlled for climbing steps and those that were represented as redundancies to perform continuous behavior. The standard sigmoidal function was used to indirectly represent the link lengths of the robot, and prevent the actual link lengths from exceeding their movable limits. A snake robot with prismatic joints was developed, and the effectiveness of the proposed method was demonstrated by experiments using the robot. Prevention of the backward movements of the trailing part by direct control of the link length and the shift motion of the connecting part, and prevention of exceeding the movable limit of the link length using the standard sigmoidal function, were confirmed.

Future studies will include the increase in the rigidity of the prismatic joints and the study of applications of the extensible degrees of freedom of the links in more complex environments.