Port-controlled Hamiltonian based control of snake robots

Abstract

This paper proposes a novel control method for path-following of joints of a snake robot. The proposed method applies the port-controlled Hamiltonian based approach, which has originally been proposed for a full-actuated friction-less system. This paper extends the controller for an under-actuated system with friction. It is proven that, by the proposed controller, the path-following of the joints is achieved. The validity of the controller is checked through simulations. Furthermore, simulation results suggest that the controller is robust to modeling errors.

Appendices

Proof of Theorem 3

It is straightforward to show that V is constant. Because of Theorem 2, we have

$$\begin{aligned} \frac{d V}{d t} = p_2^T (u_n + u_a) = p_2^T u_a. \end{aligned}$$

(40)

Therefore, we get

$$\begin{aligned} \frac{d V}{d t}&= -\beta |p_{we}^{\top } p_2 | p_2^{\top } p_{\tilde{w}} + \beta p_2^{\top } p_{\tilde{w}} \mathrm {sgn} (p_{we}^{\top }p_2) p_2^{\top } p_{we} \nonumber \\&= 0. \end{aligned}$$

(41)

Because (13) and (17) hold for any $u'$, it holds that

$$\begin{aligned} \frac{d V_{\tilde{w}}}{d t}&= \frac{d V}{d t} - \frac{d V_w}{d t} \nonumber \\&= (\alpha p_{we} + p_{\tilde{w}})^{\top } u' - \alpha \eta ^{\top } p_{\tilde{w}} - \alpha p_{we}^{\top } u' \nonumber \\&= (u'- \alpha \eta )^{\top } p_{\tilde{w}}. \end{aligned}$$

(42)

From Theorem 2, it is clear that $(u_n - \alpha \eta )^{\top } p_{\tilde{w}}=0$ holds. Therefore, if $u' = u_n + u_a$, we have

$$\begin{aligned} \frac{d V_{\tilde{w}}}{d t}&= u_a^{\top } p_{\tilde{w}} \nonumber \\&= - \beta \, |p_{we}^{\top } p_2| \, p_{\tilde{w}}^{\top } p_{\tilde{w}} + \beta p_{\tilde{w}}^{\top } p_2 \, \mathrm {sgn}(p_{we}^{\top } p_2) \, p_{we}^{\top }p_{\tilde{w}} \nonumber \\&= - \beta \, |p_{we}^{\top } p_2| \, p_{\tilde{w}}^{\top } p_{\tilde{w}} \le 0. \end{aligned}$$

(43)

Finally, we will show that the largest invariant subset of $Z := \{ (q_2, p_2) \, |\, d V_{\tilde{w}}/d t = 0 \}$ is a subset of $\{ (q_2, p_2) \, | \, V_{\tilde{w}} = 0 \}$. Because of $\{ (q_2, p_2) \, |\, d V_{\tilde{w}}/d t = 0 \} = \{ (q_2, p_2) \, |\, p_{\tilde{w}}=0 \vee p_{we}^{\top }p_2=0 \}$, it suffices to show that any subset of $\{ (q_2, p_2) \, |\, p_{\tilde{w}} \ne 0 \wedge p_{we}^{\top } p_2 = 0 \}$ is not an invariant set.

Using Theorem 2, we have

$$\begin{aligned} \frac{d}{dt} p_{we}^{\top } p_2 = \eta ^{\top } p_{\tilde{w}} + p_{we}^{\top }u' = p_{we}^{\top } u_a, \end{aligned}$$

(44)

for $u'=u_n + u_a$. Therefore, the following is obtained:

$$\begin{aligned} \frac{d}{dt} p_{we}^{\top } p_2&= - \beta \, | p_{we}^{\top } p_2 | \, p_{\tilde{w}}^{\top } p_{we} + \beta p_{\tilde{w}}^{\top } p_2 \, \mathrm {sgn}(p_{we}^{\top } p_2) \, p_{we}^{\top }p_{we} \nonumber \\&= \beta \Vert p_{\tilde{w}} \Vert ^2 \, \mathrm {sgn}(p_{we}^{\top } p_2). \end{aligned}$$

(45)

Therefore, if $p_{\tilde{w}} \ne 0$, $p_{we}^{\top } p_2$ cannot be constant. Note that we defined the signum function as (6) and it never gets 0.

As a consequence, the largest invariant subset of Z must be included in $\{ (q_2, p_2) \, | \, p_{\tilde{w}} = 0 \} = \{ (q_2, p_2) \, | \, V_{\tilde{w}} = 0 \}$ From LaSalle’s invariant principle, we conclude $V_{\tilde{w}} \rightarrow 0$ as $t \rightarrow \infty$. $\square$

Proof of Theorem 5

First, we will show that $V_{\tilde{w}} + U$ is non-increasing. The derivative of $V_{\tilde{w}} + U$ is

$$\begin{aligned} \frac{d(V_{\tilde{w}}+U)}{dt} = p_{\tilde{w}}^{\top }(u' - \alpha \eta ) + \frac{\partial U}{\partial q_2} ( \varLambda _{12}^{\top } p_1 + \varLambda _{22} p_2). \end{aligned}$$

(46)

Because (29) holds for $u' = u_n+u_a+u_p$, if $u'=u_n+u_a+u_p+u_r$, we have

$$\begin{aligned} \frac{d(V_{\tilde{w}}+U)}{dt}&= p_{\tilde{w}}^{\top } u_r - \beta \, |p_{we}^{\top } p_2 | \, p_{\tilde{w}}^{\top } p_{\tilde{w}} \nonumber \\&= - (p_{we}^{\top } p_2) \gamma \, (V+U-V_R) p_{\tilde{w}}^{\top } p_{we} \nonumber \\&\qquad \qquad - \beta \, |p_{we}^{\top } p_2 | \, p_{\tilde{w}}^{\top } p_{\tilde{w}} \nonumber \\&= - \beta \, |p_{we}^{\top } p_2 | \, p_{\tilde{w}}^{\top } p_{\tilde{w}} \le 0, \end{aligned}$$

(47)

which implies that $V_{\tilde{w}} + U$ is non-increasing.

Second, we will show the convergence to the reference path, under the conditions of the theorem. Because the equality of (47) holds only if $p_{we}^{\top } p_2 =0$ or $p_{\tilde{w}}=0$, it suffices to show the followings: any subset of $\{ (q_2, p_2) \, | \, p_{we}^{\top } p_2 = 0 \wedge p_{\tilde{w}} \ne 0 \}$ is not invariant, and the only invariant set within $\{ (q_2, p_2) \, | \, p_{\tilde{w}}=0 \}$ is $\{ (q_2, p_2) \, | \, q_2 \in C_{\mathrm {ref}} \}$.

Let us assume $p_{we}^{\top } p_2 = 0$ and $p_{\tilde{w}} \ne 0$ at some point, and consider the behavior of $p_{we}^{\top }p_2$ from that state. For $u'=u_n+u_a + u_p+u_r$, we have

$$\begin{aligned} \frac{d}{dt} (p_{we}^{\top }p_2)&= \frac{\partial \alpha }{\partial q_2} p_2 + p_{we}^{\top } \dot{p}_2 \nonumber \\&= p_{\tilde{w}}^{\top } \eta + p_{we}^{\top } (u_n + u_a + u_p + u_r) \nonumber \\&= (p_{\tilde{w}}^{\top } \eta + p_{we}^{\top } u_n) + p_{we}^{\top } (u_a + u_p + u_r) \nonumber \\&= p_{we}^{\top } (u_a + u_p + u_r), \end{aligned}$$

(48)

and

$$\begin{aligned} p_{we}^{\top } u_a&= \beta \, (p_{\tilde{w}}^{\top } p_2) \, \mathrm {sgn} (p_{we}^{\top } p_2), \end{aligned}$$

(49)

$$\begin{aligned} p_{we}^{\top } u_p&= - \frac{\partial U}{\partial q_2} \varLambda _{22} p_{we} - \xi ^{\top } p_{we} = 0, \end{aligned}$$

(50)

$$\begin{aligned} p_{we}^{\top } u_r&= - (p_{we}^{\top } p_2) \gamma \, (V + U - V_R). \end{aligned}$$

(51)

Because of $p_{we}^{\top } p_2 = 0$, we have

$$\begin{aligned} p_{we}^{\top } u_a = \beta \, (p_{\tilde{w}}^{\top } p_2), p_{we}^{\top } u_r = 0, \end{aligned}$$

(52)

which leads to

$$\begin{aligned} \frac{d}{d t} (p_{we}^{\top } p_2) = \beta (p_{\tilde{w}}^{\top } p_2) = \beta \Vert p_{\tilde{w}} \Vert ^2 > 0. \end{aligned}$$

(53)

Therefore, the state of $p_{we}^{\top } p_2 = 0$ and $p_{\tilde{w}} \ne 0$ cannot be invariant.

Next, we assume $p_{\tilde{w}}=0$ and examine the condition for this state to be invariant. The derivative of $p_{\tilde{w}} = p_2 - (p_{we}^{\top } p_2)$ is given by

$$\begin{aligned} \frac{d p_{\tilde{w}}}{d t}&= u' - \frac{d (p_{we}^{\top } p_2)}{d t} p_{we} - \alpha \frac{\partial p_{we}}{\partial q_2} \dot{q}_2, \end{aligned}$$

(54)

where $u'=u_n+u_a+u_p+u_r$. Regarding the 2nd term of the right-hand side, we have

$$\begin{aligned} \frac{d (p_{we}^{\top } p_2)}{d t}&= p_{we}^{\top } (u_a + u_p + u_r) \nonumber \\&= \beta \, p_{\tilde{w}}^{\top } p_2 \, \mathrm {sgn} (p_{we}^{\top } p_2) - p_{we}^{\top } p_2 \, \gamma \, (V + U - V_R). \end{aligned}$$

(55)

In the case of $p_{\tilde{w}}=0$, we have $u_n = u_a = 0$, $\beta \, p_{\tilde{w}}^{\top } p_2 \, \mathrm {sgn} (p_{we}^{\top } p_2) = 0$, and $\xi '=0$. Therefore, the following holds:

$$\begin{aligned} \frac{d p_{\tilde{w}}}{d t}&= u_p + u_r + p_{we}^{\top } p_2 \, \gamma \, (V + U - V_R) \nonumber \\&\qquad \qquad - \alpha \frac{\partial p_{we}}{\partial q_2} (\varLambda _{12}^{\top } p_{h} + \varLambda _{22} p_2) \nonumber \\&= \left[ u_p - \alpha \frac{\partial p_{we}}{\partial q_2} (\varLambda _{12}^{\top } p_{h} + \varLambda _{22} p_2) \right] \nonumber \\&\qquad \qquad + \left[ u_r + (p_{we}^{\top } p_2) \gamma (V + U - V_R) \right] \nonumber \\&= - \varLambda _{22} \frac{\partial U}{\partial q_2}^{\top } - \xi _0 - \xi ' + - \alpha \frac{\partial p_{we}}{\partial q_2} (\varLambda _{12}^{\top } p_{h} + \varLambda _{22} p_2) \nonumber \\&= - \varLambda _{22} \frac{\partial U}{\partial q_2}^{\top }. \end{aligned}$$

(56)

Therefore, the state of $p_{\tilde{w}} = 0$ is invariant if

$$\begin{aligned} - \varLambda _{22} \frac{\partial U}{\partial q_2}^{\top } = 0, \end{aligned}$$

(57)

which is true only if $q_2 \in C_{\mathrm {ref}}$ under the condition $p_2 \ne 0$ and $p_{\tilde{w}}=0$. Because $p_{\tilde{w}}=0$ can be invariant only if $\varLambda _{22} (\partial U / \partial q_2) = 0$, if the initial state satisfies $p_2 \ne 0$ or $\varLambda _{22} (\partial U / \partial q_2) \ne 0$, the state of $p_2=0$ is not invariant. Therefore, under the conditions of the theorem, $q_2$ approaches to the reference path $C_{\mathrm {ref}}$ and $V_{\tilde{w}}+U\rightarrow 0$ as $t \rightarrow \infty$.

Finally, we show $V + U \rightarrow V_R$ as $t \rightarrow \infty$. The derivative of $V + U - V_R$ along the trajectory of the system (12) is

$$\begin{aligned} \frac{d(V + U - V_R)}{dt}&= \frac{d(V+U)}{dt} \nonumber \\&= p_2^{\top } u' + \frac{\partial U}{\partial q_2} (\varLambda _{12}^{\top } p_1 + \varLambda _{22} p_2). \end{aligned}$$

(58)

From the proof of Theorem 4, we have $p_2^{\top } (u_n + u_a + u_p) + (\partial U / \partial q_2) (\varLambda _{12}^{\top } p_1 + \varLambda _{22} p_2) = 0$. Therefore, we get

$$\begin{aligned} \frac{d(V + U - V_R)}{dt} = p_2^{\top } u_r = - (p_{we}^{\top } p_2)^2 \gamma \, (V + U - V_R). \end{aligned}$$

(59)

Because the state of $p_{we}^{\top } p_2 = 0$ is not invariant as shown above, $(p_{we}^{\top } p_2)^2 \, \gamma$ cannot converge to 0. This is sufficient to see $\int _0^{\infty } (p_{we}^{\top } p_2)^2 \gamma dt = \infty$. Therefore, (59) suggests $V + U \rightarrow V_R$ as $t \rightarrow \infty$ $\square$