Research Article
A novel approach for modeling and tracking control of a passive-wheel snake robot
Advances in Mechanical Engineering 2017, Vol. 9(3) 1–15 Ó The Author(s) 2017 DOI: 10.1177/1687814017693944 journals.sagepub.com/home/ade
Kang Huang, Ke Shao, Shengchao Zhen and Hao Sun
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-degreeof-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. Keywords Snake robot, dynamic modeling, nonholonomic constraints, trajectory tracking, servo constraint force, under-actuated system
Date received: 10 October 2016; accepted: 22 January 2017 Academic Editor: Mario L Ferrari
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 underactuated 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 School of Mechanical Engineering, Hefei University of Technology, Hefei, China Corresponding author: Ke Shao, School of Mechanical Engineering, Hefei University of Technology, Tunxi Road 193, Hefei 230001, China. Email:
[email protected]
Creative Commons CC-BY: This article is distributed under the terms of the Creative Commons Attribution 3.0 License (http://www.creativecommons.org/licenses/by/3.0/) which permits any use, reproduction and distribution of the work without further permission provided the original work is attributed as specified on the SAGE and Open Access pages (https://us.sagepub.com/en-us/nam/ open-access-at-sage).
2 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 Tsakiris1 and Kelly and Murray2 modeled the passive-wheel snake robot using differential geometry method and discussed the controllability of the proposed model. Ostrowski3 and Ostrowski and Burdick4 provide the dynamic model of the snake and simplified it considering the symmetry of group. Prautsch and Mita5 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,10 and Toyoshima et al.11 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.15 and El Rafei et al.16 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
Advances in Mechanical Engineering 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 colleagues26–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 colleagues32–34 first made a preliminary research in the field of nonlinear mechanical systems using servo constraint control method. Chen35–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. Schutte39 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.40 focused on the mathematical calculation problems in servo control solution when using Udwadia–Kalaba equation, essential for the
Huang et al.
3
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-offreedom 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 passivewheel 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 : = (q1 q2 . . . qn )T . Using Newtonian or Lagrangian equation, the equation of motion of the unconstrained system can be written as25 _ tÞ, qð0Þ = q0 , q_ ð0Þ = q_ 0 M ðq, tÞ€ q = Qðq, q,
ð1Þ
where M(q, t) is a positive definite inertia n 3 n matrix, q_ is the n 3 1 vector of velocity, €q is the n 3 1 vector of _ t) is also an n 3 1 vector which acceleration, Q(q, q, 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 t0 are defined by q0 and q_ 0 , respectively. The generalized acceleration of the unconstrained _ t), is thus system at time t, which is defined by a(q, q, given by _ tÞ _ tÞ = €q = M 1 ðq, tÞQðq, q, aðq, q,
ð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 gi ðq, tÞ = 0,
i = 1, 2, . . . , h
ð3Þ
and m h nonholonomic constraints in the form of _ tÞ = 0, g i ðq, q,
i = h + 1, h + 2, . . . , 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 _ tÞ€q = bðq, q, _ tÞ Aðq, q,
ð5Þ
_ t) is an m 3 n matrix denoting the conwhere A(q, q, _ t) is an m 3 1 vector. straint matrix and b(q, q, 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 secondorder form, which is more flexible for manipulating the equations.37 The second-order constraints are the most suitable form for further dynamic analysis. The secondorder constraint is useful because the constraint equation is linear in acceleration. Using the secondorder 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
4
Advances in Mechanical Engineering
actual equation of motion of the constrained system can thus be written as _ tÞ _ tÞ + Qc ðq, q, M ðq, tÞ€q = Qðq, q, c
ð6Þ
n
From equations (6), (7), (11), and (12), the analytical equation of motion of general constrained mechanical system can be given by M€q = Q + M 1=2 B+ b AM 1 Q + M 1=2 I B+ B M 1=2 c
_ t) 2 R is the additional forces imposed where Q (q, q, on the system. _ t) is considered to In Lagrangian dynamics, Qc (q, q, 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.27 For a constrained mechanical system with ideal constraint as well as nonideal constraints, Udwadia describe _ t) in the form of Qc (q, q,
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
_ tÞ = Qcid ðq, q, _ tÞ + Qcnid ðq, q, _ tÞ Qc ðq, q,
and the equation of motion of the ideal constrained system is
ð7Þ
_ t) is the ideal constraint force and where Qcid (q, q, _ t) is the nonideal constraint force. Qcnid (q, q, Equation (6) is more general which is applicable for system with ideal constraints as well as nonideal con_ t) includes constraint forces that may straints. Qc (q, q, 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 _ q, t) 2 Rn which does the denote constraint force as c(q, T work W = v c in any displacement v subjecting to _ t)v = 0. Obviously, the work done by c(q, _ q, t) A(q, q, _ t), say equals to what is done by Qc (q, q, W = vT c = vT Q c
ð8Þ
which is the extended Lagrange’s form of d’Alembert’s principle. The work done by the ideal constraint force Qcid under virtual displacements is vT Qcid = 0
ð13Þ
Qc = Qci = M 1=2 B+ b AM 1 Q
M€q = Q + M 1=2 B+ b AM 1 Q
F c ðtÞ = M 1=2 B+ b AM 1 Q
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 = mI, then equation (16) simplifies F c ðtÞ = mA+ b AM 1 Q
=M
1=2
I B B M 1=2 c +
ð17Þ
and if the unconstrained acceleration M 1 Q is zero, we have equation (18) from equation (17), written as F c ðtÞ = mA+ b
ð10Þ
ð11Þ
ð18Þ
Remark 3. For equation (13), if the mass matrix M is singular, the Udwadia–Phohomsiri27 equation should be used instead of Udwadia–Kalaba29 equation to acquire the equation of motion of the constrained system, and the equation of motion becomes
½I A+ AM €q = A
and the nonideal constraint force takes the form of Qcnid
ð16Þ
ð9Þ
Udwadia and Kalaba have proved that the ideal constraint force can be given by Qcid = M 1=2 B+ b AM 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
while the work done by nonideal constraint force is vT Qcnid 6¼ 0
ð14Þ
+
Q b
ð19Þ
ð12Þ
where B = AM 1=2 , the superscript ‘‘ + ’’ denotes the Moore–Penrose generalized inverse.
The above equation is valid when the matrix [M|A]T has full rank.27
Huang et al.
5
x_ i y_ i
=
x_ y_
+ 2l
i1 X j=1
u_ j cos uj u_ j sin uj
!
! u_ i cos ui +l , u_ i sin ui ð21Þ
i = 1, 2, . . . , n
The nonholonomic lateral velocity constraints can be written as x_ i cos ui y_ i sin ui = 0,
i = 1, 2, . . . , n
ð22Þ
Combining equation (21) and (22), we have
Figure 1. Modeling of the snake.
lu_ i + 2l
Dynamics of the snake
i1 X
u_ j cos ui uj + x_ cos ui y_ sin ui = 0,
j=1
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 Ifx, yg is assumed as an inertial frame. We also assume the generalized coordinates q = (u1 u2 u3 . . . un xy)T to describe the motion condition of the snake, where (xy)T denotes the coordinates of the snake’s head and u = (u1 u2 u3 . . . un )T denotes the absolute angle of link 1, link 2, and link 3 of the body, with respect to y-axis. p = (p1 p2 p3 . . . pn1 )T is the relative angle of each two links. We also use mi (xi yi )T to denote the mass center of each link (located on the center of each link) and its corresponding coordinates. pi represents the servo joint force imposed on each joint. Here, i = 1, 2, 3, and the length of each link is 2l.
ð23Þ
i = 1, 2, . . . , n
From equation (5), differentiating equation (23) with respect to time t once, we obtain equation (24) l€ui + 2l
i1 X
€uj cos ui uj + u_ j u_ j u_ i sin ui uj
j=1
u_ i x_ sin ui + €x cos ui u_ i y_ cos ui €y sin ui = 0, i = 1, 2, . . . , n
ð24Þ
The nonholonomic constraints can thus be written in the matrix form, given by _ q, tÞ An ðq, tÞ€q = bn ðq,
ð25Þ
where 2
l
0
0
6 l 0 6 2l cosðu2 u1 Þ 6 6 l An = 6 2l cosðu3 u1 Þ 2l cosðu3 u1 Þ 6 4 2l cosðun u1 Þ 2l cosðun u2 Þ 2l cosðun u3 Þ 0 1 f1 ðui , x, yÞ B f2 ðui , x, yÞ C C B B C bn = B f3 ðui , x, yÞ C, i = 1, 2, . . . , n B C @ A ...
cos u1
cos u2 cos u3
l
cos un
sin u1
3
7 sin u2 7 7 sin u3 7 7 7 5 sin un
fn ðui , x, yÞ 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 (xi yi )T and (xy)T is given by
xi yi
=
x y
+ 2l
i1 X sin uj j=1
cos uj
+l
sin ui cos ui
where fi (ui , x, y) denotes the function of element of b1 . 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
,
ð20Þ
i = 1, 2, . . . , n Differentiating the above equation with respect to time t once, we obtain
T=
n 1X mi x_ 2i + y_ 2i + Ii u_ 2i , 2 1
i = 1, 2, . . . , n ð26Þ
From the Lagrangian equation, the equation of motion of system without lateral velocity constraints can be given by
6
Advances in Mechanical Engineering d ∂T ∂T = Fc dt ∂q_ ∂q
ð27Þ
where
Substituting equation (21) into equation (27), we have _ q, tÞ + F c ðtÞ ð28Þ M ðq, tÞ€q = Qðq, T C t is the generalized forces where F c = Dt = 0 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 ui to the relative angle pi , t = (t1 t2 t3 . . . tn1 )T denotes the actual constraint force of the joint, and 0 is a 2 3 n 2 1 vector representing the generalized forces imposed on the snake’s head (obviously, the value equals zero). The angle converting matrix C satisfies
where
n X d ∂Al ðq, tÞ ∂Al ðq, tÞ Al ðq, tÞ = q_ k + dt ∂q ∂t k k =1
ð33Þ
and
The second-order form of the constraints can be rewritten as S€d :
n X
Ali ðq, tÞ€qi =
i=1
_ q, tÞ, = : bi ðq,
n X d d Ali ðq, tÞ€qi Al ðq, tÞ dt dt i=1
ð34Þ
l = 1, . . . , m
or in the matrix form _ q, tÞ S€d : As ðq, tÞ€q = bs ðq,
ð35Þ T
2
1 1 0 6 0 1 1 Cn1 3 n = 4 ... ... ... 0 0 0
0 0 ... 1
where As = (Ali )m 3 n and bs = (b1 b2 bm ) .
3
0 0 7 ... 5 1
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 Sd of a mechanical system can be modeled as n X
Ali ðq, tÞq_ i + Al ðq, tÞ = 0,
d Ali ðq, tÞq_ i + dt i=1
Remark 5. In fact, the second-order form constraint equation (35) is a very general form. It includes typical constraints as illustrated by Rosenberg19 (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 ð36Þ
A€q = b where
where 1 m n and Ali ( ) and Al 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 secondorder form. Differentiating constraint equation (30) with respect to t yields n X
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 secondorder 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.
l = 1, . . . , m ð30Þ
i=1
S€d :
ð32Þ
ð29Þ
p = Cu
Sd :
n X d ∂Ali ðq, tÞ ∂Ali ðq, tÞ Ali ðq, tÞ = q_ k + dt ∂q ∂t k k =1
n X i=1
Ali ðq, tÞ€qi +
d Al ðq, tÞ = 0 dt ð31Þ
A A= n As
and b =
bn bs
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
Huang et al.
7
Figure 2. Control system of the snake robot.
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 t in equation (28) can be given by t = D+ F c
ð39Þ
where f (g, t; r) is an m 3 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:
ð37Þ
the servo joint constraint force can thus be solved by
+ b AM 1 Q t = D+ M 1=2 AM 1=2
_ tÞ = f ðg, t; rÞ gðq, q,
1. 2.
g = 0 is an equilibrium point of the system. This equilibrium point is globally asymptotically stable.
ð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 t can be constructed using equation (37). The complete snake system control diagram is summarized in Figure 2, where qa and 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. _ = q_ 0 are The initial conditions q(0) = q0 and q(0) 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.32 According to the Lyapunov stability theory, the following differential equation can be constructed
Usually, numerous systems fulfilling the above two conditions can be constructed, such as g_ i = ri gi , where the constant ri .0, i = 1, 2, . . . , m. If the m desired trajectory constraints are holonomic in the form of gi ðq, tÞ = 0,
i = 1, 2, . . . , m
ð40Þ
these constraints can be modified as g€i + li g_ i + mi gi = 0,
i = 1, 2, . . . , m
ð41Þ
with li , mi .0, so that the equilibrium solution of the system g = 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 method41 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
+ b AM 1 Q t 0 = B+ M 1=2 AM 1=2 1 1 K e = t + te aMAT AAT
ð42Þ
8
Advances in Mechanical Engineering
where t = B+ M 1=2 (AM 1=2 )+ (b AM 1 Q) is the nominal control input (equation (38)) and te : = aMAT (AAT )1 K 1 e is the supplementary control input to conquer the uncertainties, where a is a _ q, t) positive constant, K 2 Rm 3 m , K.0, and e = (q, denotes the trajectory tracking error.
Table 1. Parameters of the snake for simulation.
Simulation
Name
Value
m m m l
Mass of the link 1 Mass of the link 2 Mass of the link 3 Length of each link
0.3 kg 0.3 kg 0.3 kg 0.2 m
1 1 0 , 0 1 1 t1 the actual servo joint constraint forces t = , and t2 2 3 1 0 6 1 1 7 6 1 7 the servo constraint matrix D = 6 0 7. 4 0 0 5
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 _ q, tÞ A3 ðq, tÞ€q = b3 ðq,
Parameter
where the angle converting matrix C =
ð43Þ
0
0
where 2
l
6 A3 = 4
0 cos u1
0
sin u1
3
7 0 cos u2 sin u2 5 2l cosðu1 u2 Þ l 2l cosðu1 u3 Þ 2l cosðu1 u3 Þ l cos u3 sin u3 0 1 u_ 1 x_ sin u1 + u_ 1 y_ cos u1 B C u_ 2 x_ sin u2 + u_ 2 y_ cos u2 2lu_ 1 u_ 1 u_ 2 sinðu1 u2 Þ b3 = @ A u_ 3 x_ sin u3 + u_ 3 y_ cos u3 2lu_ 1 u_ 1 u_ 3 sinðu1 u3 Þ 2lu_ 2 u_ 2 u_ 3 sinðu2 u3 Þ
From equations (26)–(29), the dynamic equation of motion of the three-link snake robot without lateral velocity constraints can be given by _ q, tÞ + F c ðtÞ M ðq, tÞ€q = Qðq,
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.
ð44Þ
where 2
2
4ðm1 + 3m2 + 3m3 Þ l3
6 6 2ðm2 + 2m3 Þcosðu1 u2 Þl2 6 6 M =6 2m3 cosðu1 u3 Þl2 6 6 4 ðm1 + 2m2 + 2m3 Þcos u1 l
2ðm2 + 2m3 Þcosðu1 u2 Þl2
2m3 cosðu1 u3 Þl2
ðm1 + 2m2 + 2m3 Þcos u1 l
ðm1 + 2m2 + 2m3 Þsin u1 l
4ðm2 + 3m3 Þ l3
2m3 cosðu2 u3 Þl2
ðm2 + 2m3 Þcos u2 l
ðm2 + 2m3 Þsin u2 l
2m3 cosðu2 u3 Þl2 ðm2 + 2m3 Þcos u2 l
4m3 l3
m3 cos u3 l
m3 sin u3 l
m3 cos u3 l m3 sin u3 l 1
m1 + m2 + m3 0
0 m1 + m2 + m3
2
ðm1 + 2m2 + 2m3 Þsin u1
2
ðm2 + 2m3 Þsin u2 l 0 2ðm2 + 2m3 Þsinðu1 u2 Þu_ 22 l2 2m3 sinðu1 u3 Þu_ 23 l2 B C 2m3 sinðu2 u3 Þu_ 23 l2 + 2ðm2 + 2m3 Þsinðu1 u2 Þu_ 21 l2 B C B C 2 2 2 2 C _ l 2m3 sinðu2 u3 Þu_ l Q=B 2m sin ð u u Þ u 3 1 3 1 2 B C B C @ ðm1 + 2m2 + 2m3 Þsin u1 u_ 21 l + ðm2 + 2m3 Þsin u2 u_ 22 l + m3 sin u3 u_ 23 l A 2 2 2 ðm1 + 2m2 + 2m3 Þcos u1 u_ l + ðm2 + 2m3 Þcos u2 u_ l + m3 cos u3 u_ l 1
2
2
F c = Dt =
CT t 0
1 6 1 6 =6 0 4 0 0
3 0 1 7 7 1 7 t1 0 5 t2 0
7 7 7 7 7 7 7 5
3
Simulation 1. We assume the snake’s head to track a circle to achieve a certain tracking task. The desired trajectory Sd is given by
and
3
ð45Þ
8
p 3 > : y= sin t 10 2
ð46Þ
Huang et al.
9
Figure 3. Calculated position of the snake’s head versus time (circle trajectory).
Table 2. Initial conditions for simulation 1. (u1 )t = 0 0 (u_ 1 )t = 0 0
(u2 )t = 0 0 (u_ 2 )t = 0 0
(u3 )t = 0 0 (u_ 3 )t = 0 0
xt = 0 0.3 x_ t = 0 0
yt = 0 0 y_ t = 0 3p=20
Differentiating constraint equation (46) with respect to t once and twice, respectively, yields 8
p 3p > < x_ = sin t 20 2 S_ d :
p 3p > : y_ = cos t 20 2
ð47Þ
8
p 3p2 > > cos t < €x = 40 2 S€d :
p 2 > 3p > : €y = sin t 40 2
ð48Þ
and
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 = t0 = 0 in equations (46) and (47), given by Table 2. It is clear that the initial conditions satisfy the nonholonomic constraint equation (23). 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. 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 e1 = x xd and e2 = y yd , where d is the desired head’s position (equation (46)). The tracking error ei , i = 1, 2, is in the order of 108 m, which indicates an excellent tracking performance. 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.
Simulation 2. We also assume the snake’s head to track a sine curve to achieve a certain tracking task, given by 8 1 > > > : y = sin t 2
ð49Þ
Differentiating (49) with respect to t once and twice, respectively, we have 8 1 > > < x_ = 2 S_ d : 1 1 > > : y_ = cos t 2 2
ð50Þ
10
Advances in Mechanical Engineering
Figure 4. Calculated required servo joint forces versus time (circle trajectory).
Table 3. Initial conditions for simulation 2. (u1 )t = 0 5p=4 (u_ 1 )t = 0 0
(u2 )t = 0 5p=4 (u_ 2 )t = 0 0
(u3 )t = 0 5p=4 (u_ 3 )t = 0 0
xt = 0 0 x_ t = 0 1/2
yt = 0 0 y_ t = 0 1/2
and S€d :
€x = 0 €y = 14 sin 12 t
ð51Þ
The initial conditions satisfying the constraints are given in Table 3. 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. 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 e1 = x xd and e2 = y yd , where d denotes the desired head’s position (equation (46)), are really small. Specifically, the tracking error ei , i = 1, 2, is in the order of 1014 and 108 m, respectively. 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.
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.
Huang et al.
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).
11
12
Advances in Mechanical Engineering 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.
Figure 6. Calculated/desired trajectory of the snake’s head (circle trajectory).
Figure 7. Calculated position of the snake’s head versus time (sine trajectory).
Figure 8. Calculated required servo joint forces versus time (sine trajectory).
Huang et al.
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).
13
14
Advances in Mechanical Engineering
9.
10.
11.
12. Figure 10. Calculated/desired trajectory of the snake’s head (sine trajectory).
Declaration of conflicting interests
13.
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article. 14.
Funding The author(s) received no financial support for the research, authorship, and/or publication of this article.
15.
References 1. Krishnaprasa P and Tsakiris D. G-snakes: nonholonomic kinematic chains on lie groups. In: Proceedings of the IEEE international conference on decision and control, Lake Buena Vista, FL, 14–16 December 2000, pp.613– 618. New York: IEEE. 2. Kelly S and Murray RM. Geometric phases and robotic locomotion. J Robotic Syst 1995; 12: 417–431. 3. Ostrowski JP. The mechanics and control of undulatory robotic locomotion. PhD Dissertation, California Institute of Technology, Pasadena, CA, 1996. 4. Ostrowski JP and Burdick J. The geometric mechanics of undulatory robotic locomotion. Int J Robot Res 1998; 17: 683–701. 5. Prautsch P and Mita T. Control and analysis of the gait of snake robots. In: Proceedings of IEEE international conference on control applications, Kohala Coast, HI, 22– 27 August 1999, pp.502–507. New York: IEEE. 6. Date H, Hoshi Y and Sampei M. Locomotion control of a snake-like robot based on dynamic manipulability. In: Proceedings of IEEE/RSJ international conference on intelligent robots and systems, Taipei, 18–22 October 2000, pp.2236–2241. New York: IEEE. 7. Date H, Hoshi Y and Sampei M. Locomotion control of a snake robot with constraint force attenuation. In: Proceedings of the American control conference, Arlington, VA, 25–27 June 2001, pp.113–118. New York: IEEE. 8. Watanabe K, Iwase M and Hatakeyama S. Control strategy for a snake-like robot based on constraint force and verification by experiment. In: Proceedings of the
16.
17.
18.
19. 20.
21.
22. 23. 24.
IEEE/RSJ international conference on intelligent robots and systems, Nice, 22–26 September 2008, pp.1618–1623. New York: IEEE. Watanabe K, Iwase M and Hatakeyama S. Control strategy for a snake-like robot based on constraint force and verification by experiment. Adv Robotics 2009; 23: 907–937. Hicks G and Ito K. A method for determination of optimal gaits with application to a snake-like serial-link structure. IEEE T Automat Contr 2005; 50: 1291–1306. Toyoshima S, Tanaka M and Matsuno F. A study on sinus-lifting motion of a snake robot with sequential optimization of a hybrid system. IEEE T Autom Sci Eng 2014; 11: 139–144. Liljeback P, Haugstuen IU and Pettersen KY. Path following control of planar snake robots using a cascaded approach. In: Proceedings of IEEE conference on decision and control, Atlanta, GA, 15–17 December 2010, pp.1969–1976. New York: IEEE. Liljeback P, Haugstuen IU and Pettersen KY. Experimental investigation of a path following controller for planar snake robots. In: Proceedings of IEEE conference on decision and control, Singapore, 7–10 December 2010, pp.2325–2332. New York: IEEE. Liljeback P and Pettersen KY. Waypoint guidance control of snake robots. In: Proceedings of IEEE conference on decision and control, Shanghai, China, 9–13 May 2011, pp.937–944. New York: IEEE. Alamir M, El Rafei M, Hafidi G, et al. Feedback design for 3D movement of an Eel-like robot. In: Proceedings of IEEE conference on decision and control, Roma, 10–14 April 2007, pp.256–261. New York: IEEE. El Rafei M, Alamir M, Marchand N, et al. Motion control of a three-dimensional eel-like robot without pectoral fins. In: Proceedings of the 17th world congress, the international federation of automatic control, 2008, pp.750–755, https://hal.archives-ouvertes.fr/hal-00199433/document Guo X, ShuGen MA, Bin L, et al. Modelling and optimal torque control of a snake-like robot based on the fiber bundle theory. Sci China Inf Sci 2015; 58: 032205-1–032205-13. Guo X, ShuGen MA, Bin L, et al. Locomotion control of a snakelike robot based on velocity disturbance. In: Proceedings of 2014 IEEE international conference on robotics and biomimetics, Bali, Indonesia, 5–10 December 2014, pp.582–587. New York: IEEE. Rosenberg RM. Analytical dynamics of discrete systems. New York: Plenum Press, 1977. Neimark JI and Fufaev NA. Dynamics of nonholonomic systems. Providence, RI: American Mathematical Society, 1972. Baumgarte J. Stabilization of constraints and integrals of motion in dynamical systems. Comput Method Appl M 1972; 1: 1–16. Hamel G. Die lagrange-eulersche gleichungen der mechanik. Z Math Phys 1904; 50: 1–57 (in German). Slotine JJE and Li WP. Applied nonlinear control. Upper Saddle River, NJ: Prentice Hall, 1991. Appell P. Sur une forme generale des equations de la dynamique. Comptes R Acad Sci 1899; 129: 459–460.
Huang et al. 25. Gibbs JW. On the fundamental formulae of dynamics. Am J Math 1879; 2: 49–64. 26. Udwadia FE and Koganti PB. Dynamics and control of a multi-body planar pendulum. Nonlinear Dynam 2015; 81: 845–866. 27. Udwadia FE and Phohomsiri P. Explicit equations of motion for constrained mechanical systems with singular mass matrices and applications to multi-body dynamics. P Roy Soc A: Math Phy 2006; 462: 2097–2117. 28. Udwadia FE and Kalaba RE. What is the general form of the explicit equations of motion for constrained mechanical systems. J Appl Mech 2002; 69: 335–339. 29. Udwadia FE and Kalaba RE. Explicit equations of motion for mechanical systems with non-ideal constraints. J Appl Mech 2001; 68: 462–467. 30. Udwadia FE and Kalaba RE. Analytical dynamics: a new approach. Cambridge: Cambridge University Press, 1996. 31. Udwadia FE and Kalaba RE. A new perspective on constrained motion. P Roy Soc A: Math Phy 1992; 439: 407–410. 32. Udwadia FE. A new perspective on the tracking control of nonlinear structural and mechanical systems. P Roy Soc Lond A Mat 2003; 459: 1783–1800. 33. Koganti PB, Udwadia FE and Koganti PB. Synthesis of stable optimal controls from Lyapunov based constraints. In: Proceedings of the ASME 2013 international design engineering technical conferences and computers and information in engineering conference, Portland, OR, 4–7 August 2013. New York: ASME.
15 34. Udwadia FE and Koganti PB. Optimal stable control for nonlinear dynamical systems: an analytical dynamics based approach. Nonlinear Dynam 2015; 82: 547–562. 35. Chen YH. Mechanical systems under servo constraints: the Lagrange’s approach. Mechatronics 2005; 15: 317–337. 36. Chen YH. Equations of motion of mechanical systems: given force depends on constraint force. Mechatronics 1999; 9: 411–428. 37. Chen YH. Second-order constraints for equations of motion of constrained systems. IEEE/ASME T Mech 1998; 3: 240–248. 38. Chen YH. Equations of motion of mechanical systems under servo constraints: the Maggi approach. Mechatronics 2008; 18: 208–217. 39. Schutte AD. Permissible control of general constrained mechanical systems. J Frankl Inst 2010; 347: 208–227. 40. Bajodah AH, Hodges DH and Chen YH. Inverse dynamics of servo-constraints based on the generalized inverse. Nonlinear Dynam 2005; 39: 179–196. 41. Chen YH and Zhang X. Adaptive robust approximate constraint-following control for mechanical systems. J Frank Inst 2010; 347: 69–86. 42. Chen YH. Constraint-following servo control design for mechanical systems. J Vib Control 2009; 15: 369–389. 43. Chen YH. Approximate constraint-following of mechanical systems under uncertainty. Nonlinear Dynam Syst Theor 2008; 8: 329–337.
