A novel approach for modeling and tracking control of a passive-wheel snake robot

Abstract

This article proposed a novel approach for the dynamic modeling and controller design of a passive-wheel snake robot based on Udwadia–Kalaba theory. Compared to common methods, this approach is easily processed for many-degree-of-freedom snake robot. The desired trajectory is easily modeled as a trajectory constraint, and the nonholonomic constraints from the passive wheels’ lateral velocity are also easily handled using Udwadia–Kalaba theory. Besides the proposed equation of motion is analytical, no approximation or linearization as well as extra variables such as Lagrangian multiplier is needed as common methods usually do. The servo joint constraint forces are precisely calculated by solving Udwadia–Kalaba equation, and no complicated control structure design is needed as common control methods always do especially for under-actuated mechanical systems. This article provides a novel dynamic modeling and controller design approach for the passive-wheel snake robot in a new perspective. The theoretical analysis and simulation verify the proposed approach. The trajectory has good incidence with the designed one, and the real-time joint forces are also conveniently acquired.

Introduction

As a type of bionic robots, the snake robot is drawing attention in recent years. The snake robot can do a number of works such as environment detection, resource prospecting, and pipeline maintenance, which is difficult or dangerous for human beings to process. In the above application fields, the snake is usually controlled to fulfill a desired motion.

Because of the nonlinear, nonholonomic, and under-actuated characteristics of the passive-wheel snake system, the modeling and control is always difficult to handle. On one hand, the modeling quality of the snake system has significant influence on the control performance of the snake. As a many-degree-of-freedom mechanical system, the dynamic modeling of the passive-wheel snake robot is complicated and difficult to process using classical dynamic theory such as Newton equation and Lagrangian equation which are mostly used in the snake’s modeling. Moreover, approximation and linearization are always used when using common methods to model the snake which detects the modeling accuracy. Besides, it is always not easy to model the passive wheels’ nonholonomic constraints. Currently, there are mainly two ways to handle these constraints. First, the passive wheels’ constraints are modeled by a “geometry equality method,” that is, the nonholonomic constraint imposed on the snake body. Second, one can also use the “force equality method” to model these constraints, in which the passive wheel is simulated by an infinite lateral force and a zero-longitudinal force. However, most of the current dynamic theories show difficulty when dealing with such constraints.

On the other hand, as an under-actuated mechanical system, the trajectory tracking control of the snake robot is always difficult and complicated to handle. Commonly, common methods always make assumptions of the model which detects the control accuracy. Moreover, extra procedure variables, such as Lagrangian multiplier, make the controller design difficult and complex. Besides, the modified controller structure such as added visual sensors which is processed in the controller design procedure when using common control methods also makes the controller design a heavy task.

Many researchers have done much work on the dynamic modeling and controller design of the snake robot. Krishnaprasad and Tsakiris ¹ and Kelly and Murray ² modeled the passive-wheel snake robot using differential geometry method and discussed the controllability of the proposed model. Ostrowski ³ and Ostrowski and Burdick ⁴ provide the dynamic model of the snake and simplified it considering the symmetry of group. Prautsch and Mita ⁵ modeled a plat snake robot using Lagrangian equation and studied its tracking control performance based on Lyapunov theory. Date et al.,^6,7 Watanabe et al.,^8,9 Hicks and Ito, ¹⁰ and Toyoshima et al. ¹¹ modeled the snake and studied the optimized torque control of the system on the basis of classical dynamic theories. Lijieback et al.^12–14 achieved the trajectory tracking controller design for the snake robot. Alamir et al. ¹⁵ and El Rafei et al. ¹⁶ designed feedback control scheme and sliding mode control scheme for snake-like eel robot. Guo et al.^17,18 utilized the fiber bundle theory to realize the snake’s modeling and researched the optimal torque control of the snake.

However, all the above methods are mathematically difficult and complicated, either making approximation or linearization of the dynamic model or making a modified structure of the controller, which detects the modeling quality and the control performance. In this article, we propose a novel approach of the modeling and controller design of the passive-wheel snake robot on the basis of the Udwadia–Kalaba theory. Compared to common methods, this approach provides analytical equation of motion of the snake system, in which the nonholonomic wheel constraints are easily modeled. Besides, this approach is easy to handle, especially applicable for the modeling of many-degree-of-freedom snake systems, without extra process variables such as Lagrangian multiplier. Based on the proposed Udwadia–Kalaba fundamental equation of motion, the controller is also designed handily without any extra structures of the controller. The servo constraint forces of the joints are acquired exactly by solving the Udwadia–Kalaba equation, and the snake can move exactly in excellent coincident with the desired trajectory.

Classical dynamic theories for constraint mechanical system are derived on the basis of d’Alembert’s principle and virtual displacement principle, such as Newton–Euler equation, Lagrangian equation, Boltzmann equation, Maggie equation, and Hamel equation.^19–25 Because of the assumptions in the above theory, as well as the conditions for many-degree-of-freedom mechanical systems, the applications of the classical dynamic theory turn out not optimistic. Besides, the lack of flexibility of the classical dynamic theories makes it complicated and difficult to model a practical mechanical system, especially for a complex mechanical system with different types of constraints.

On the basis of former researches, Udwadia and colleagues^26–31 provided fundamental equation of motion of constraint mechanical systems. Compared to classical dynamic theories, the equation of motion is easily acquired mainly by three steps, without any other variables such as Lagrangian multiplier in Lagrangian equation, which is handy to process. Moreover, the equation is in more exact analytical form, because of no linearization or application in the derivation. Besides, this approach can deal with ideal or nonideal constraints and holonomic or nonholonomic constraints, applicable also for over-actuated or under-actuated systems. Moreover, the number of coordinates turns out less important, as long as the constraint equations are consistent and the mass matrix is positive definite. The Udwadia–Kalaba theory brings a new perspective for the trajectory tracking problem.

In analytical dynamics, the trajectory tracking control problem is also defined as a problem of servo constraint control. Udwadia and colleagues^32–34 first made a preliminary research in the field of nonlinear mechanical systems using servo constraint control method. Chen^35–38 achieved the systematical servo constraint controller design of mechanical systems based on Udwadia’s research and also studied servo constraint problem, indicating that based on Maggi equation, the required constraint inputs can be acquired by servo control. Schutte ³⁹ studied the modeling and control problem of nonholonomic mechanical systems when using Udwadia–Kalaba equation and designed nonlinear feedback controllers for such types of mechanical system, and Bajodah et al. ⁴⁰ focused on the mathematical calculation problems in servo control solution when using Udwadia–Kalaba equation, essential for the practical application of this equation. Chen et al.^41–43 also researched on the adaptive robust control of uncertain mechanical systems on the basis of Udwadia–Kalaba theory.

It is useful to handle the dynamic modeling and control problem based on Udwadia–Kalaba theory. First, the modeling is easy to process, consists of mainly three steps, especially for the modeling of a many-degree-of-freedom snake robot. Not only the more accurate analytical equation of motion can be acquired, without using any extra process variables such as Lagrangian multiplier, but also the nonholonomic lateral velocity constraints can also be handily modeled. The desired trajectory is treated as a constraint named trajectory constraint in this approach, which is conveniently modeled in the Udwadia–Kalaba equation. Second, based on the proposed dynamic model, the controller design can be derived directly, no linearization or approximation of the model or modification of the controller is needed such as visual sensors as common methods usually do, especially for the under-actuated nonholonomic passive-wheel snake robot system. The real-time joint constraint forces can be acquired easily by solving Udwadia–Kalaba equation, which is efficient for the snake’s exact motion control.

This article proposed a novel approach for the dynamic modeling and controller design of the passive-wheel snake robot based on Udwadia–Kalaba theory. The nonholonomic wheel constraints are handily modeled in the Udwadia–Kalaba fundamental equation of motion. This approach makes the modeling process of the many-degree-of-freedom snake robot system simplified largely by mainly three steps, without any extra process variables. The equation of motion is analytical, without any simplification of the system as common methods usually do, which is significant for the motion performance of the snake. The controller is also designed on the basis on the proposed dynamic model. The servo joint constraint forces of the snake are acquired exactly by solving the equation of motion, and the calculated trajectory is of good coincident with the desired one. The controller is easy to achieve because of no extra structure design of the control system, which is always complicated when using common methods where extra structures such as visual sensors are essential.

Dynamics of Udwadia–Kalaba theory

First consider the unconstrained mechanical system with the n generalized coordinates, denoted by $q:=(q_1{q}_2\dots q_n{)}^T$ . Using Newtonian or Lagrangian equation, the equation of motion of the unconstrained system can be written as ²⁵

$$M(q,t)\stackrel{··}{q}=Q(q,\stackrel{·}{q},t),q\left(0\right)=q_0,\stackrel{·}{q}\left(0\right)={\stackrel{·}{q}}_0$$

(1)

where $M(q,t)$ is a positive definite inertia $n\times n$ matrix, $\stackrel{·}{q}$ is the $n\times 1$ vector of velocity, $\stackrel{··}{q}$ is the $n\times 1$ vector of acceleration, $Q(q,\stackrel{·}{q},t)$ is also an $n\times 1$ vector which denotes the imposed force imposed on the system. The imposed forces can include centrifugal force, gravitational force, control input, and so on. The initial conditions at time $t_0$ are defined by $q_0$ and ${\stackrel{·}{q}}_0$ , respectively.

The generalized acceleration of the unconstrained system at time t, which is defined by $a(q,\stackrel{·}{q},t)$ , is thus given by

$$a(q,\stackrel{·}{q},t)=\stackrel{··}{q}=M^{-1}(q,t)Q(q,\stackrel{·}{q},t)$$

(2)

Second, constraints imposed on the system are considered. The constrained system is assumed to be subjected to h holonomic constraints in the form of

$${\gamma }_i(q,t)=0,i=1,2,\dots ,h$$

(3)

and $m-h$ nonholonomic constraints in the form of

$${\gamma }_i(q,\stackrel{·}{q},t)=0,i=h+1,h+2,\dots ,m$$

(4)

The constraint equations must be consistent so that we do not care whether the constraints are linearly independent or not in this article.

The constraint matrix equation can be acquired by differentiating holonomic constraints (equation (3)) twice and nonholonomic constraints (equation (4)) once with respect to time t, respectively, and is given by

$$A(q,\stackrel{·}{q},t)\stackrel{··}{q}=b(q,\stackrel{·}{q},t)$$

(5)

where $A(q,\stackrel{·}{q},t)$ is an $m\times n$ matrix denoting the constraint matrix and $b(q,\stackrel{·}{q},t)$ is an $m\times 1$ vector.

Remark 1

As we know, constraint equation used in classical dynamics such as Lagrangian equation, Maggi equation, and Gibbs and Appell equation is either in the zero-order or first-order form. While the constraint equation of Udwadia–Kalaba equation is in a second-order form, which is more flexible for manipulating the equations. ³⁷ The second-order constraints are the most suitable form for further dynamic analysis. The second-order constraint is useful because the constraint equation is linear in acceleration. Using the second-order constraints of fundamental equation of motion, one can handily analyze discontinuous acceleration problems. Furthermore, the second-order form constraints can also be used to prove the Gauss’ principle.

By combining the unconstrained equation of motion (1) and the constraint equation (5), the analytical Udwadia–Kalaba equation of motion of the constrained mechanical system is finally acquired. Additional generalized constraint forces resulting from the constraints should be imposed on the system. The actual equation of motion of the constrained system can thus be written as

$$M(q,t)\stackrel{··}{q}=Q(q,\stackrel{·}{q},t)+Q^c(q,\stackrel{·}{q},t)$$

(6)

where $Q^c(q,\stackrel{·}{q},t)\in R^n$ is the additional forces imposed on the system.

In Lagrangian dynamics, $Q^c(q,\stackrel{·}{q},t)$ is considered to be ideal governed by d’Alembert’s principle which indicates that forces of constraint do zero work under virtual displacements. However, the constraints can also be nonideal in actual applications. The nonideal constraints generate nonideal constraint forces such as friction force and electromagnetic force, which usually has significant effects on the system performances. ²⁷ For a constrained mechanical system with ideal constraint as well as nonideal constraints, Udwadia describe $Q^c(q,\stackrel{·}{q},t)$ in the form of

$$Q^c(q,\stackrel{·}{q},t)=Q_{\mathrm{id}}^c(q,\stackrel{·}{q},t)+Q_{\mathrm{nid}}^c(q,\stackrel{·}{q},t)$$

(7)

where $Q_{\mathrm{id}}^c(q,\stackrel{·}{q},t)$ is the ideal constraint force and $Q_{\mathrm{nid}}^c(q,\stackrel{·}{q},t)$ is the nonideal constraint force.

Equation (6) is more general which is applicable for system with ideal constraints as well as nonideal constraints. $Q^c(q,\stackrel{·}{q},t)$ includes constraint forces that may do positive, negative, or zero work under virtual displacement at any instant time during the motion of the constrained system, as shown in equation (7). We denote constraint force as $c(\stackrel{·}{q},q,t)\in R^n$ which does the work $W=v^{T}c$ in any displacement v subjecting to $A(q,\stackrel{·}{q},t)v=0$ . Obviously, the work done by $c(\stackrel{·}{q},q,t)$ equals to what is done by $Q^c(q,\stackrel{·}{q},t)$ , say

$$W=v^{T}c=v^T{Q}^c$$

(8)

which is the extended Lagrange’s form of d’Alembert’s principle. The work done by the ideal constraint force $Q_{\mathrm{id}}^c$ under virtual displacements is

$$v^T{Q}_{\mathrm{id}}^c=0$$

(9)

while the work done by nonideal constraint force is

$$v^T{Q}_{\mathrm{nid}}^c\ne 0$$

(10)

Udwadia and Kalaba have proved that the ideal constraint force can be given by

$$Q_{\mathrm{id}}^c=M^{1/2}{B}^{+}(b-A{M}^{-1}Q)$$

(11)

and the nonideal constraint force takes the form of

$$Q_{\mathrm{nid}}^c=M^{1/2}(I-B^{+}B)M^{-1/2}c$$

(12)

where $B=A{M}^{-1/2}$ , the superscript “+” denotes the Moore–Penrose generalized inverse.

From equations (6), (7), (11), and (12), the analytical equation of motion of general constrained mechanical system can be given by

$$M\stackrel{··}{q}=Q+M^{1/2}{B}^{+}(b-A{M}^{-1}Q)+M^{1/2}(I-B^{+}B)M^{-1/2}c$$

(13)

where the vector c is determined by the mechanician and can be obtained by observation or experimentation.

Equation (13) is called the Udwadia–Kalaba fundamental equation of motion. When c equals zero, equation (12) reduces to d’Alembert’s principle, which means the total work done under virtual displacement is zero from equation (8). For an ideal constrained system, we thus have

$$Q^c=Q_i^c=M^{1/2}{B}^{+}(b-A{M}^{-1}Q)$$

(14)

and the equation of motion of the ideal constrained system is

$$M\stackrel{··}{q}=Q+M^{1/2}{B}^{+}(b-A{M}^{-1}Q)$$

(15)

Therefore, the required additional constraint force $F^c(t)$ at each instant of time t of the constrained system is given by

$$F^c\left(t\right)=M^{1/2}{B}^{+}(b-A{M}^{-1}Q)$$

(16)

Remark 2

$F^c(t)$ in equation (16) is the control inputs that we desired to achieve. Here, if the matrix M is a constant diagonal matrix so that $M=\mathrm{mI}$ , then equation (16) simplifies

$$F^c\left(t\right)=m{A}^{+}(b-A{M}^{-1}Q)$$

(17)

and if the unconstrained acceleration $M^{-1}Q$ is zero, we have equation (18) from equation (17), written as

$$F^c\left(t\right)=m{A}^{+}b$$

(18)

Remark 3

For equation (13), if the mass matrix M is singular, the Udwadia–Phohomsiri ²⁷ equation should be used instead of Udwadia–Kalaba ²⁹ equation to acquire the equation of motion of the constrained system, and the equation of motion becomes

$$\stackrel{··}{q}={\left[\begin{array}{c}[I-A^{+}A]M\hfill \\ \multicolumn{1}{c}{A}\end{array}\right]}^{+}\left[\begin{array}{c}Q\hfill \\ \multicolumn{1}{c}{b}\end{array}\right]$$

(19)

The above equation is valid when the matrix [M|A]T has full rank. ²⁷

Dynamics of the snake

As is shown in Figure 1, we design the earth fixed frame I to study the modeling and control of the n-link passive-snake robot which moves on a flat ground. The earth fixed frame $I\{x,y\}$ is assumed as an inertial frame. We also assume the generalized coordinates $q=({\theta }_1{\theta }_2{\theta }_3\dots {\theta }_{n}xy{)}^T$ to describe the motion condition of the snake, where ${(xy)}^T$ denotes the coordinates of the snake’s head and $\theta =({\theta }_1{\theta }_2{\theta }_3\dots {\theta }_n{)}^T$ denotes the absolute angle of link 1, link 2, and link 3 of the body, with respect to y-axis. $p=(p_1{p}_2{p}_3\dots p_{n-1}{)}^T$ is the relative angle of each two links. We also use $m_i(x_i{y}_i{)}^T$ to denote the mass center of each link (located on the center of each link) and its corresponding coordinates. $p_i$ represents the servo joint force imposed on each joint. Here, i = 1, 2, 3, and the length of each link is 2l.

Figure 1.

Modeling of the snake.

The lateral velocity of each wheel equals to be zero to mimic the friction characteristic of snake’s abdomen. For an n-link snake robot, the relationship between $(x_i{y}_i{)}^T$ and $(xy{)}^T$ is given by

$$\begin{array}{l}\left(\begin{array}{c}{x}_i\\ y_i\end{array}\right)=\left(\begin{array}{c}x\\ y\end{array}\right)+2l{\displaystyle \sum _{j=1}^{i-1}\left(\begin{array}{c}sin{\theta }_j\\ cos{\theta }_j\end{array}\right)}+l\left(\begin{array}{c}sin{\theta }_i\\ cos{\theta }_i\end{array}\right),\\ i=1,2,\mathrm{\dots },n\end{array}$$

(20)

Differentiating the above equation with respect to time t once, we obtain

$$\begin{array}{l}\left(\begin{array}{c}{\stackrel{.}{x}}_i\\ {\stackrel{.}{y}}_i\end{array}\right)=\left(\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\end{array}\right)+2l{\displaystyle \sum _{j=1}^{i-1}\left(\begin{array}{c}{\stackrel{.}{\theta }}_{j}cos{\theta }_j\\ -{\stackrel{.}{\theta }}_{j}sin{\theta }_j\end{array}\right)}+l\left(\begin{array}{c}{\stackrel{.}{\theta }}_{i}cos{\theta }_i\\ -{\stackrel{.}{\theta }}_{i}sin{\theta }_i\end{array}\right),\\ i=1,2,\mathrm{\dots },n\end{array}$$

(21)

The nonholonomic lateral velocity constraints can be written as

$${\stackrel{·}{x}}_i\cos {\theta }_i-{\stackrel{·}{y}}_i\sin {\theta }_i=0,i=1,2,\dots ,n$$

(22)

Combining equation (21) and (22), we have

$$\begin{array}{l}l{\stackrel{.}{\theta }}_i+2l{\displaystyle \sum _{j=1}^{i-1}{\stackrel{.}{\theta }}_j}cos\left({\theta }_i-{\theta }_j\right)+\stackrel{.}{x}cos{\theta }_i-\stackrel{.}{y}sin{\theta }_i=0,\\ i=1,2,\mathrm{\dots },n\end{array}$$

(23)

From equation (5), differentiating equation (23) with respect to time t once, we obtain equation (24)

$$\begin{array}{l}l{\stackrel{··}{\theta }}_i+2l{\displaystyle \sum _{j=1}^{i-1}\left\{{\stackrel{··}{\theta }}_j\cos \left({\theta }_i-{\theta }_j\right)+{\stackrel{.}{\theta }}_j\left({\stackrel{.}{\theta }}_j-{\stackrel{.}{\theta }}_i\right)\sin \left({\theta }_i-{\theta }_j\right)\right\}}\\ -{\stackrel{.}{\theta }}_i\stackrel{.}{x}sin{\theta }_i+\stackrel{··}{x}cos{\theta }_i-{\stackrel{.}{\theta }}_i\stackrel{.}{y}cos{\theta }_i-\stackrel{··}{y}sin{\theta }_i=0,\\ i=1,2,\mathrm{\dots },n\end{array}$$

(24)

The nonholonomic constraints can thus be written in the matrix form, given by

$$A_n(q,t)\stackrel{··}{q}=b_n(\stackrel{·}{q},q,t)$$

(25)

where

$$\begin{array}{c}{A}_n=\left[\begin{array}{ccccccc}l\hfill & 0\hfill & 0\hfill & \cdots \hfill & \cdots \hfill & \cos {\theta }_1\hfill & -\sin {\theta }_1\hfill \\ \multicolumn{1}{c}{2l\cos ({\theta }_2-{\theta }_1)}& l\hfill & 0\hfill & \cdots \hfill & \cdots \hfill & \cos {\theta }_2\hfill & -\sin {\theta }_2\hfill \\ \multicolumn{1}{c}{2l\cos ({\theta }_3-{\theta }_1)}& 2l\cos ({\theta }_3-{\theta }_1)\hfill & l\hfill & \cdots \hfill & \cdots \hfill & \cos {\theta }_3\hfill & -\sin {\theta }_3\hfill \\ \multicolumn{1}{c}{\cdots }& \cdots \hfill & \cdots \hfill & \cdots \hfill & \cdots \hfill & \cdots \hfill & \cdots \hfill \\ \multicolumn{1}{c}{2l\cos ({\theta }_n-{\theta }_1)}& 2l\cos ({\theta }_n-{\theta }_2)\hfill & 2l\cos ({\theta }_n-{\theta }_3)\hfill & \cdots \hfill & l\hfill & \cos {\theta }_n\hfill & -\sin {\theta }_n\hfill \end{array}\right]\hfill \\ b_n=\left(\begin{array}{c}{f}_1({\theta }_i,x,y)\hfill \\ \multicolumn{1}{c}{f_2({\theta }_i,x,y)}\\ \multicolumn{1}{c}{\begin{array}{c}{f}_3({\theta }_i,x,y)\hfill \\ \multicolumn{1}{c}{\dots }\\ \multicolumn{1}{c}{f_n({\theta }_i,x,y)}\end{array}}\end{array}\right),i=1,2,\dots ,n\hfill \end{array}$$

where $f_i({\theta }_i,x,y)$ denotes the function of element of $b_1$ .

Now, we derive the equation of motion of the snake without the lateral velocity constraints using Lagrangian equation. The system’s kinetic energy can be given by

$$T=\frac{1}{2}\sum _1^n\{m_i({\stackrel{·}{x}}_i^2+{\stackrel{·}{y}}_i^2)+I_i{\stackrel{·}{\theta }}_i^2\},i=1,2,\dots ,n$$

(26)

From the Lagrangian equation, the equation of motion of system without lateral velocity constraints can be given by

$$\frac{d}{\mathrm{dt}}\frac{\partial T}{\partial \stackrel{·}{q}}-\frac{\partial T}{\partial q}=F^c$$

(27)

Substituting equation (21) into equation (27), we have

$$M(q,t)\stackrel{··}{q}=Q(\stackrel{·}{q},q,t)+F^c\left(t\right)$$

(28)

where $F^c=D\tau =\left(\begin{array}{c}{C}^T\tau \hfill \\ \multicolumn{1}{c}{0}\end{array}\right)$ is the generalized forces imposed on the system, D is the servo constraint matrix which is determined by the structure of the available servo controls, C is the angle converting matrix from the absolute angle ${\theta }_i$ to the relative angle $p_i$ , $\tau =({\tau }_1{\tau }_2{\tau }_3\dots {\tau }_{n-1}{)}^T$ denotes the actual constraint force of the joint, and 0 is a 2 × n − 1 vector representing the generalized forces imposed on the snake’s head (obviously, the value equals zero). The angle converting matrix C satisfies

$$p=C\theta$$

(29)

where

$$C_{n-1\times n}=\left[\begin{array}{ccc}\begin{array}{ccc}-1\hfill & 1\hfill & 0\hfill \end{array}\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{\begin{array}{ccc}0\hfill & -1\hfill & 1\hfill \end{array}}& 0\hfill & 0\hfill \\ \multicolumn{1}{c}{\begin{array}{ccc}\begin{array}{c}\dots \hfill \\ \multicolumn{1}{c}{0}\end{array}\hfill & \begin{array}{c}\dots \hfill \\ \multicolumn{1}{c}{0}\end{array}\hfill & \begin{array}{c}\dots \hfill \\ \multicolumn{1}{c}{0}\end{array}\hfill \end{array}}& \begin{array}{c}\dots \hfill \\ \multicolumn{1}{c}{-1}\end{array}\hfill & \begin{array}{c}\dots \hfill \\ \multicolumn{1}{c}{1}\end{array}\hfill \end{array}\right]$$

Generally, the head of the snake is designed to track a desired trajectory to accomplish a certain tracking task. The problem of constrained motion in analytical dynamics can be reframed as a trajectory tracking control problem, based on Udwadia–Kalaba theory. And the desired trajectories are treated as constraints named trajectory tracking constraints in Udwadia–Kalaba theory. The desired trajectory, say, the trajectory tracking constraint, can be written in the form of constant equation (5), and the servo constraint force $F^c(t)$ can thus be redefined as the control input we desire to apply to the system to realize the trajectory tracking control.

The desired trajectory tracking constraints $S_d$ of a mechanical system can be modeled as

$$S_d:\sum _{i=1}^n{A}_{\mathrm{li}}(q,t){\stackrel{·}{q}}_i+A_l(q,t)=0,l=1,\dots ,m$$

(30)

where $1\le m\le n$ and $A_{\mathrm{li}}(·)$ and $A_l$ are both $C^1$ in q and t. These constraints imply restrictions on the velocities as well as the displacement and are the first-order form of the constraints.

Now, we convert the constraints into the second-order form. Differentiating constraint equation (30) with respect to t yields

$${\stackrel{··}{S}}_d:\sum _{i=1}^n\frac{d}{\mathrm{dt}}{A}_{\mathrm{li}}(q,t){\stackrel{·}{q}}_i+\sum _{i=1}^n{A}_{\mathrm{li}}(q,t){\stackrel{··}{q}}_i+\frac{d}{\mathrm{dt}}{A}_l(q,t)=0$$

(31)

where

$$\frac{d}{\mathrm{dt}}{A}_{\mathrm{li}}(q,t)=\sum _{k=1}^n\frac{\partial A_{\mathrm{li}}(q,t)}{\partial q_k}{\stackrel{·}{q}}_k+\frac{\partial A_{\mathrm{li}}(q,t)}{\partial t}$$

(32)

and

$$\frac{d}{\mathrm{dt}}{A}_l(q,t)=\sum _{k=1}^n\frac{\partial A_l(q,t)}{\partial q_k}{\stackrel{·}{q}}_k+\frac{\partial A_l(q,t)}{\partial t}$$

(33)

The second-order form of the constraints can be rewritten as

$$\begin{array}{l}{\stackrel{··}{S}}_d:{\displaystyle \sum _{i=1}^n{A}_{li}}\left(q,t\right){\stackrel{··}{q}}_i=-{\displaystyle \sum _{i=1}^n\frac{d}{dt}}{A}_{li}\left(q,t\right){\stackrel{··}{q}}_i-\frac{d}{dt}{A}_l\left(q,t\right)\\ =:b_i\left(\stackrel{.}{q},q,t\right),l=1,\mathrm{\dots },m\end{array}$$

(34)

or in the matrix form

$${\stackrel{··}{S}}_d:A_s(q,t)\stackrel{··}{q}=b_s(\stackrel{·}{q},q,t)$$

(35)

where $A_s=(A_{\mathrm{li}}{)}_{m\times n}$ and $b_s=(b_1{b}_2\cdots b_m{)}^T$ .

Remark 4

Similarly, if a constraint equation is given in the zero-order form, differentiate it with respect to t twice, and if a constraint equation has one-order form, differentiate it with respect to t once, and the second-order form constraint equations can thus be acquired. All one needs to do is to prescribe a desirable system constraint and then convert it to a second-order form.

Remark 5

In fact, the second-order form constraint equation (35) is a very general form. It includes typical constraints as illustrated by Rosenberg ¹⁹ (1997). It also includes a number of standard control problems such as stabilization, trajectory following, and optimality.^36,37 The trajectory tracking constraint is just one of these constraints above.

Combining equations (25) and (35), the constraint of the snake system can thus be written as

$$A\stackrel{··}{q}=b$$

(36)

where

$$A=\left[\begin{array}{c}{A}_n\hfill \\ \multicolumn{1}{c}{A_s}\end{array}\right]\mathrm{and}b=\left(\begin{array}{c}{b}_n\hfill \\ \multicolumn{1}{c}{b_s}\end{array}\right)$$

Controller design

The joint constraint force $F^c(t)$ should be applied to realize the trajectory tracking control target of the snake head according to Udwadia–Kalaba equation. The controller design can be derived directly, without the linearization of the nonlinear equation of motion or the imposition of an a priori structure on the nature of the controller as in many commonly used methods.

The joint servo constraint force $F^c$ can be written as equation (16) and the actual servo control input $\tau$ in equation (28) can be given by

$$\tau =D^{+}{F}^c$$

(37)

the servo joint constraint force can thus be solved by

$$\tau =D^{+}{M}^{1/2}{\left(A{M}^{-1/2}\right)}^{+}(b-A{M}^{-1}Q)$$

(38)

where the superscript “+” denotes the Moore–Penrose generalized inverse.

Remark 6

One should determine the servo structure by choosing D to obtain $F^c$ in equation (37) for a particular mechanical system. Then, the dynamic system is formulated in the form of equation (6). By solving the control input $F^c$ based on equation (16), the actual servo control input $\tau$ can be constructed using equation (37).

The complete snake system control diagram is summarized in Figure 2, where $q_a$ and ${\stackrel{·}{q}}_a$ denote the actual displacement and velocity of the snake, respectively. We can see from Figure 2 that the tracking controller based on Udwadia–Kalaba theory is really simple, requiring very little computation compared with commonly used methods, especially useful for the online real-time control of the snake.

Figure 2.

Control system of the snake robot.

The initial conditions $q(0)=q_0$ and $\stackrel{·}{q}(0)={\stackrel{·}{q}}_0$ are required to satisfy the constraints (36). However, it is usually difficult to achieve in practical engineering applications due to various factors. The Lyapunov stability theory can be sued to handle this problem. ³²

According to the Lyapunov stability theory, the following differential equation can be constructed

$$g(q,\stackrel{·}{q},t)=-f(g,t;r)$$

(39)

where $f(g,t;r)$ is an $m\times 1$ vector and r is a parameter vector related to the system’s dynamic characteristic. $f(g,t;r)$ is chosen so that the system has the following two conditions:

1. $g=0$ is an equilibrium point of the system.

2. This equilibrium point is globally asymptotically stable.

Usually, numerous systems fulfilling the above two conditions can be constructed, such as ${\stackrel{·}{g}}_i=-r_i{g}_i$ , where the constant $r_i>0,i=1,2,\dots ,m$ . If the m desired trajectory constraints are holonomic in the form of

$$g_i(q,t)=0,i=1,2,\dots ,m$$

(40)

these constraints can be modified as

$${\stackrel{··}{g}}_i+{\lambda }_i{\stackrel{·}{g}}_i+{\mu }_i{g}_i=0,i=1,2,\dots ,m$$

(41)

with ${\lambda }_i,{\mu }_i>0$ , so that the equilibrium solution of the system $g=\stackrel{·}{g}=0$ is asymptotically stable.

Remark 7

In reality, there are a number of uncertainties in practical systems, such as the initial positions, the external disturbances, and the parameter variations. The constraint stability method ⁴¹ is useful for handling these uncertainties to achieve the exact adaptive robustness control of the system.

Based on Udwadia–Kalaba equation, the control input (38) can be rewritten as

$$\begin{array}{l}\tau \prime ={\mathrm{B}}^{+}{\mathrm{M}}^{1/2}{\left({\mathrm{AM}}^{-1/2}\right)}^{+}\left({\mathrm{b}-\mathrm{AM}}^{-1}\mathrm{Q}\right)\\ -\alpha {\mathrm{MA}}^{\mathrm{T}}{\left({\mathrm{AA}}^{\mathrm{T}}\right)}^{-1}{\mathrm{K}}^{-1}\mathrm{e}=\tau +{\tau }_{\mathrm{e}}\end{array}$$

(42)

where $\tau =B^{+}{M}^{1/2}(A{M}^{-1/2}{)}^{+}(b-A{M}^{-1}Q)$ is the nominal control input (equation (38)) and ${\tau }_e:=-\alpha M{A}^T(A{A}^T{)}^{-1}{K}^{-1}e$ is the supplementary control input to conquer the uncertainties, where $\alpha$ is a positive constant, $K\in R^{m\times m}$ , $K>0$ , and $e=(\stackrel{·}{q},q,t)$ denotes the trajectory tracking error.

Simulation

We start with the dynamic modeling and the controller design of a three-link snake robot using the proposed approach derived from Udwadia–Kalaba theory.

For n = 3, from equation (20–25), we have the lateral velocity constraints

$$A_3(q,t)\stackrel{··}{q}=b_3(\stackrel{·}{q},q,t)$$

(43)

where

$$\begin{array}{c}{A}_3=\left[\begin{array}{ccc}\begin{array}{cc}l\hfill & 0\hfill \\ \multicolumn{1}{c}{2l\cos ({\theta }_1-{\theta }_2)}& l\hfill \end{array}\hfill & \begin{array}{cc}0\hfill & \cos {\theta }_1\hfill \\ \multicolumn{1}{c}{0}& \cos {\theta }_2\hfill \end{array}\hfill & \begin{array}{c}-\sin {\theta }_1\hfill \\ \multicolumn{1}{c}{-\sin {\theta }_2}\end{array}\hfill \\ \multicolumn{1}{c}{\begin{array}{cc}2l\cos ({\theta }_1-{\theta }_3)\hfill & 2l\cos ({\theta }_1-{\theta }_3)\hfill \end{array}}& \begin{array}{cc}l\hfill & \cos {\theta }_3\hfill \end{array}\hfill & -\sin {\theta }_3\hfill \end{array}\right]\hfill \\ b_3=\left(\begin{array}{c}{\stackrel{·}{\theta }}_1\stackrel{·}{x}\sin {\theta }_1+{\stackrel{·}{\theta }}_1\stackrel{·}{y}\cos {\theta }_1\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\theta }}_2\stackrel{·}{x}\sin {\theta }_2+{\stackrel{·}{\theta }}_2\stackrel{·}{y}\cos {\theta }_2-2l{\stackrel{·}{\theta }}_1({\stackrel{·}{\theta }}_1-{\stackrel{·}{\theta }}_2)\sin ({\theta }_1-{\theta }_2)}\\ \multicolumn{1}{c}{{\stackrel{·}{\theta }}_3\stackrel{·}{x}\sin {\theta }_3+{\stackrel{·}{\theta }}_3\stackrel{·}{y}\cos {\theta }_3-2l{\stackrel{·}{\theta }}_1({\stackrel{·}{\theta }}_1-{\stackrel{·}{\theta }}_3)\sin ({\theta }_1-{\theta }_3)-2l{\stackrel{·}{\theta }}_2({\stackrel{·}{\theta }}_2-{\stackrel{·}{\theta }}_3)\sin ({\theta }_2-{\theta }_3)}\end{array}\right)\hfill \end{array}$$

From equations (26)–(29), the dynamic equation of motion of the three-link snake robot without lateral velocity constraints can be given by

$$M(q,t)\stackrel{··}{q}=Q(\stackrel{·}{q},q,t)+F^c\left(t\right)$$

(44)

where

$$\begin{array}{c}M=\left[\begin{array}{ccccc}4(m_1+3m_2+3m_3)\frac{l^2}{3}\hfill & 2(m_2+2m_3)\cos ({\theta }_1-{\theta }_2)l^2\hfill & 2m_3\cos ({\theta }_1-{\theta }_3)l^2\hfill & (m_1+2m_2+2m_3)\cos {\theta }_{1}l\hfill & -(m_1+2m_2+2m_3)\sin {\theta }_{1}l\hfill \\ \multicolumn{1}{c}{2(m_2+2m_3)\cos ({\theta }_1-{\theta }_2)l^2}& 4(m_2+3m_3)\frac{l^2}{3}\hfill & 2m_3\cos ({\theta }_2-{\theta }_3)l^2\hfill & (m_2+2m_3)\cos {\theta }_{2}l\hfill & -(m_2+2m_3)\sin {\theta }_{2}l\hfill \\ \multicolumn{1}{c}{2m_3\cos ({\theta }_1-{\theta }_3)l^2}& 2m_3\cos ({\theta }_2-{\theta }_3)l^2\hfill & 4m_3\frac{l^2}{3}\hfill & m_3\cos {\theta }_{3}l\hfill & -m_3\sin {\theta }_{3}l\hfill \\ \multicolumn{1}{c}{(m_1+2m_2+2m_3)\cos {\theta }_{1}l}& (m_2+2m_3)\cos {\theta }_{2}l\hfill & m_3\cos {\theta }_{3}l\hfill & m_1+m_2+m_3\hfill & 0\hfill \\ \multicolumn{1}{c}{-(m_1+2m_2+2m_3)\sin {\theta }_1}& -(m_2+2m_3)\sin {\theta }_{2}l\hfill & -m_3\sin {\theta }_{3}l\hfill & 0\hfill & m_1+m_2+m_3\hfill \end{array}\right]\hfill \\ Q=\left(\begin{array}{c}-2(m_2+2m_3)\sin ({\theta }_1-{\theta }_2){\stackrel{·}{\theta }}_2^2{l}^2-2m_3\sin ({\theta }_1-{\theta }_3){\stackrel{·}{\theta }}_3^2{l}^2\hfill \\ \multicolumn{1}{c}{-2m_3\sin ({\theta }_2-{\theta }_3){\stackrel{·}{\theta }}_3^2{l}^2+2(m_2+2m_3)\sin ({\theta }_1-{\theta }_2){\stackrel{·}{\theta }}_1^2{l}^2}\\ \multicolumn{1}{c}{2m_3\sin ({\theta }_1-{\theta }_3){\stackrel{·}{\theta }}_1^2{l}^2-2m_3\sin ({\theta }_2-{\theta }_3){\stackrel{·}{\theta }}_2^2{l}^2}\\ \multicolumn{1}{c}{(m_1+2m_2+2m_3)\sin {\theta }_1{\stackrel{·}{\theta }}_1^{2}l+(m_2+2m_3)\sin {\theta }_2{\stackrel{·}{\theta }}_2^{2}l+m_3\sin {\theta }_3{\stackrel{·}{\theta }}_3^{2}l}\\ \multicolumn{1}{c}{(m_1+2m_2+2m_3)\cos {\theta }_1{\stackrel{·}{\theta }}_1^{2}l+(m_2+2m_3)\cos {\theta }_2{\stackrel{·}{\theta }}_2^{2}l+m_3\cos {\theta }_3{\stackrel{·}{\theta }}_3^{2}l}\end{array}\right)\hfill \end{array}$$

and

$$F^c=D\tau =\left(\begin{array}{c}{C}^T\tau \hfill \\ \multicolumn{1}{c}{0}\end{array}\right)=\left[\begin{array}{cc}-1\hfill & 0\hfill \\ \multicolumn{1}{c}{1}& -1\hfill \\ \multicolumn{1}{c}{\begin{array}{c}0\hfill \\ \multicolumn{1}{c}{0}\\ \multicolumn{1}{c}{0}\end{array}}& \begin{array}{c}1\hfill \\ \multicolumn{1}{c}{0}\\ \multicolumn{1}{c}{0}\end{array}\hfill \end{array}\right]\left(\begin{array}{c}{\tau }_1\hfill \\ \multicolumn{1}{c}{{\tau }_2}\end{array}\right)$$

(45)

where the angle converting matrix $C=\left[\begin{array}{ccc}-1\hfill & 1\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& -1\hfill & 1\hfill \end{array}\right]$ , the actual servo joint constraint forces $\tau =\left(\begin{array}{c}{\tau }_1\hfill \\ \multicolumn{1}{c}{{\tau }_2}\end{array}\right)$ , and the servo constraint matrix $D=\left[\begin{array}{cc}-1\hfill & 0\hfill \\ \multicolumn{1}{c}{1}& -1\hfill \\ \multicolumn{1}{c}{\begin{array}{c}0\hfill \\ \multicolumn{1}{c}{0}\\ \multicolumn{1}{c}{0}\end{array}}& \begin{array}{c}1\hfill \\ \multicolumn{1}{c}{0}\\ \multicolumn{1}{c}{0}\end{array}\hfill \end{array}\right]$ .

Based on equations (44) and (45), the servo joint constraint forces can be acquired by solving equation (38). Table 1 shows the parameter values of the snake for simulation.

Table 1.

Parameters of the snake for simulation.

Parameter	Name	Value
m	Mass of the link 1	0.3 kg
m	Mass of the link 2	0.3 kg
m	Mass of the link 3	0.3 kg
l	Length of each link	0.2 m

Simulation 1

We assume the snake’s head to track a circle to achieve a certain tracking task. The desired trajectory $S_d$ is given by

$$S_d:\{\begin{array}{c}\multicolumn{1}{c}{x=\frac{3}{10}\cos \left(\frac{\pi }{2}t\right)}\\ \multicolumn{1}{c}{y=\frac{3}{10}\sin \left(\frac{\pi }{2}t\right)}\end{array}$$

(46)

Differentiating constraint equation (46) with respect to t once and twice, respectively, yields

$${\stackrel{·}{S}}_d:\{\begin{array}{c}\stackrel{·}{x}=-\frac{3\pi }{20}\sin \left(\frac{\pi }{2}t\right)\hfill \\ \stackrel{·}{y}=\frac{3\pi }{20}\cos \left(\frac{\pi }{2}t\right)\hfill \end{array}$$

(47)

and

$${\stackrel{··}{S}}_d:\{\begin{array}{c}\multicolumn{1}{c}{\stackrel{··}{x}=-\frac{3{\pi }^2}{40}\cos \left(\frac{\pi }{2}t\right)}\\ \multicolumn{1}{c}{\stackrel{··}{y}=-\frac{3{\pi }^2}{40}\sin \left(\frac{\pi }{2}t\right)}\end{array}$$

(48)

In this article, the initial conditions satisfy the desired trajectory tracking constraint equations for the simplification of simulation.

The initial conditions containing initial displacement and velocity of the snake can be calculated by letting $t=t_0=0$ in equations (46) and (47), given by Table 2. It is clear that the initial conditions satisfy the nonholonomic constraint equation (23).

Table 2.

Initial conditions for simulation 1.

$({\theta }_1{)}_{t=0}$	$({\theta }_2{)}_{t=0}$	$({\theta }_3{)}_{t=0}$	$x_{t=0}$	$y_{t=0}$
0	0	0	0.3	0
$({\stackrel{·}{\theta }}_1{)}_{t=0}$	$({\stackrel{·}{\theta }}_2{)}_{t=0}$	$({\stackrel{·}{\theta }}_3{)}_{t=0}$	${\stackrel{·}{x}}_{t=0}$	${\stackrel{·}{y}}_{t=0}$
0	0	0	0	$3\pi /20$

We use ode45 integrator in MATLAB to process the simulation of the proposed approach. The simulation time is 15 s. Figure 3 shows the calculated trajectory of the snake’s head as functions of time t, where x and y represent the displacements of x-axis and y-axis of the head, respectively. We can see clearly from Figure 3 that the position is well coincident with the desired motion (46). By solving equation of motion (38), the required real-time servo joint forces are acquired as Figure 4 describes.

Figure 3.

Calculated position of the snake’s head versus time (circle trajectory).

Figure 4.

Calculated required servo joint forces versus time (circle trajectory).

A more detailed tracking performance of the numerical errors of head’s position as functions of time is given in Figure 5. From Figure 5, we can see that the error between the calculated position and the desired, say, the trajectory tracking constraint equations (46), is small enough. Specifically, the trajectory tracking error is given by $e_1=x-x_d$ and $e_2=y-y_d$ , where ${*}_d$ is the desired head’s position (equation (46)). The tracking error $e_i$ , i = 1, 2, is in the order of ${10}^{-8}\mathrm{m}$ , which indicates an excellent tracking performance.

Figure 5.

Position error of the snake’s head versus time: (a) position error of x direction versus time and (b) position error of y direction versus time (circle trajectory).

Figure 6 shows a much more intuitive description of the trajectory tracking performance. Figure 6 shows the calculated and desired trajectory of the snake’s head. We can also conclude that the tracking error is really small that the two trajectories have a good coincidence.

Figure 6.

Calculated/desired trajectory of the snake’s head (circle trajectory).

Simulation 2

We also assume the snake’s head to track a sine curve to achieve a certain tracking task, given by

$$S_d:\{\begin{array}{c}x=\frac{1}{2}t\hfill \\ y=\sin \left(\frac{1}{2}t\right)\hfill \end{array}$$

(49)

Differentiating (49) with respect to t once and twice, respectively, we have

$${\stackrel{·}{S}}_d:\{\begin{array}{c}\stackrel{·}{x}=\frac{1}{2}\hfill \\ \stackrel{·}{y}=\frac{1}{2}\cos \left(\frac{1}{2}t\right)\hfill \end{array}$$

(50)

and

$${\stackrel{··}{S}}_d:\{\begin{array}{c}\stackrel{··}{x}=0\hfill \\ \multicolumn{1}{c}{\stackrel{··}{y}=-\frac{1}{4}\sin \left(\frac{1}{2}t\right)}\end{array}$$

(51)

The initial conditions satisfying the constraints are given in Table 3.

Table 3.

Initial conditions for simulation 2.

$({\theta }_1{)}_{t=0}$	$({\theta }_2{)}_{t=0}$	$({\theta }_3{)}_{t=0}$	$x_{t=0}$	$y_{t=0}$
$5\pi /4$	$5\pi /4$	$5\pi /4$	0	0
$({\stackrel{·}{\theta }}_1{)}_{t=0}$	$({\stackrel{·}{\theta }}_2{)}_{t=0}$	$({\stackrel{·}{\theta }}_3{)}_{t=0}$	${\stackrel{·}{x}}_{t=0}$	${\stackrel{·}{y}}_{t=0}$
0	0	0	1/2	1/2

The calculated position is given in Figure 7. As shown in Figure 7, the head’s position coincides well with the desired motion (49). The calculated servo joint forces are displayed in Figure 8.

Figure 7.

Calculated position of the snake’s head versus time (sine trajectory).

Figure 8.

Calculated required servo joint forces versus time (sine trajectory).

The tracking errors of head’s position as functions of time are given in Figure 9. We can see clearly from Figure 9 that the tracking errors $e_1=x-x_d$ and $e_2=y-y_d$ , where ${*}_d$ denotes the desired head’s position (equation (46)), are really small. Specifically, the tracking error $e_i$ , i = 1, 2, is in the order of ${10}^{-14}$ and ${10}^{-8}\mathrm{m}$ , respectively.

Figure 9.

Position error of the snake’s head versus time: (a) position error of x direction versus time and (b) position error of y direction versus time (sine trajectory).

An intuitive description of the trajectory tracking performance is shown in Figure 10. It is clear from Figure 10 that the tracking error is small enough, indicating an excellent tracking performance.

Figure 10.

Calculated/desired trajectory of the snake’s head (sine trajectory).

Conclusion

In this article, a novel approach for the trajectory tracking control of a passive-wheel snake robot is proposed based on Udwadia–Kalaba theory. Nonlinear dynamic model of the snake is derived, and a desired trajectory is designed to illustrate this approach. The theoretical analysis and MATLAB simulation results indicate that the servo constraint control based on Udwadia–Kalaba equation fulfills the trajectory tracking task of the snake. The servo constraint torques are obtained conveniently, and the snake head movement meets the designed trajectory precisely.

The snake is a classical many-degree-of-freedom under-actuated mechanical system with characteristics of nonlinearization and nonholonomic constraints and that its dynamic modeling and controller design is relatively difficult using common methods. This article provides a novel perspective for the modeling and control problem of the passive-snake robot. The main contributions of this article are as follows:

1. The modeling process is easy to handle, very useful in dynamic modeling of a snake, and the constraints can be easily modeled in the analytical Udwadia–Kalaba fundamental equation. The constraints can be holonomic as well as nonholonomic, and what we need to do is just differentiate the constraints (3) and (4) with respect to time t once or twice and transform it into the form of equation (5), more efficient compared to common methods. The desired trajectories are treated as constraints named trajectory tracking constraints in Udwadia–Kalaba theory, which can be written in the form of equation (5). The passive wheels’ nonholonomic constraints can also be easily modeled by differentiating the constraints once with respect to time t.

2. Although many researchers proposed modified control methods to improve the tracking performances of the passive-wheel snake, these methods either make linearization or approximation of the nonlinear system or use extra variables such as Lagrangian multiplier or impose a priori structure on the nature of the controller, which is complicated and inefficient, and the tracking performance is always not optimistic. Using Udwadia–Kalaba theory, analytical equation of motion is acquired, it is exact, and no approximation of the equation of motion or modification of the controller is needed, producing an excellent control performance.