Modeling and Motion Control of a Double-Pendulum ...

1

Modeling and Motion Control of a Double-Pendulum Driven Cart Yang Liu and Hongnian Yu Faculty of Computing, Engineering and Technology, Staffordshire University, Stafford, ST16 0DG UK [email protected] Abstract—The significant advantage of ground mobile robots (GMRs) is the accessibility to the area that is inaccessible for human. One difficulty of current GMR technology is that the external driving mechanisms, such as wheel, track, and leg, restrict its applications. In order to solve this limitation, this paper proposes a novel driving mechanism using a novel GMR called the double-pendulum driven cart (DPDC). The DPDC that only employs internal thrust and static friction is capable of steering by two parallel pendulums mounted on the cart, and is able to implement planar elemental motions by designing the pendulum trajectory properly. This paper initially studies the dynamic modeling and the motion control issues of the DPDC. A closed-loop controller using the partial feedback linearization technique is designed for the tracking control of the parallel pendulums to guarantee the accurate motion of the cart. Extensive simulation results verify the proposed model and the efficiency of the motion control strategy. Keywords—Mobile robot, double-pendulum, pendulum driven, underactuated system, motion control I. INTRODUCTION A ground mobile robot (GMR) is a robotic device that is capable of moving on ground or rough environment. One significant advantage of GMRs is the accessibility to the area that is inaccessible for human. This priority benefits GMRs in a number of applications that include, but not limit to space exploration [1][2], rescue and recovery [3][4], and medical endoscopy [5][6]. During the past decade, abundant research on GMRs has focused on kinematic and dynamic modeling [7], motion and tracking control [8][9], localization and navigation [10]. In general, there are three types of GMRs: wheeled mobile robots, tracked mobile robots, and legged mobile robots. They are respectively using active wheel, track, and leg as driving methods. Corresponding to the classification, there are three significant real-life issues for which the research of GMRs has become a hotspot of robotics: 1) A demand on the autonomous device, which is dexterous to move on the ground, can serve disabled and elder people.

2

2) A demand on the robust device, which is adaptable to highly dynamic environments, can be operated in a broad range of conditions, such as cragged and muddy terrains. 3) A demand on the mimic device, which imitates the motion of insect or animal, can be employed in the environments which are inaccessible by human. This paper mainly addresses the third point which inspires from the motion of a snake [11] and applies the idea to a four-wheel passive cart mounted with two parallel pendulums. However, a comprehensive modeling and motion control strategy are critical for the design and implementation of the proposed mechanism. Therefore, this paper only considers the study of the rationale, the dynamic modeling, and the motion control strategy of the DPDC system. With the increase of new applications, conventional GMRs cannot satisfy the exigent and specific requirements from industry and healthcare. One difficulty of current GMR technology is that the external driving mechanisms, such as wheel, track, and leg restrict its applications. Simi et al. proposed a novel design for the active capsule endoscopy using an internal actuating legged mechanism which interacts with an external magnetic field in [5]. In [12], Glass et al. presented an actuated legged robot with an anchoring mechanism to enhance the existing capsule endoscopes. Kim et al. developed a locomotive capsule endoscope integrated with four external clampers based on the bio-mimetic approach in [6]. But however, these driving mechanisms are external which are harmful to the gastrointestinal tract that contacts with the mechanism. This issue more or less motivates the originality of this paper. Therefore, this paper aims to present a new concept of locomotion for the GMR which only employs internal thrust and static force, but without any assistance of the external driving mechanisms. The DPDC is an extended system of the single-pendulum driven cart (SPDC) that has been studied for one-dimensional locomotion in [13][14]. The DPDC system proposed in this paper has four passive wheels and two parallel pendulums mounted on the pivots of the cart. By rotating the pendulums, internal thrusts are generated at the pivots which can drive the cart on a plane. The difference between the DPDC and the conventional GMR is that the former steers via internal thrust, while the latter steers via wheel rotation. The advantage of the proposed concept is that the wheels on the DPDC are negligible which will enhance its agility and flexibility, and can be applied in an uncertain environment, for example, a capsule endoscope, which causes many issues by the external driving mechanisms. Extended the SPDC system, the contribution of the DPDC system is that it extends the motion space of the proposed driving concept from one dimension (forward and backward motion) to two dimensions (planar motion). This paper is devoted to studying the dynamic modeling and the motion control strategy of the DPDC system, and aims to demonstrate its capability to implement basic trajectories on a plane. In summary,

3

this paper has the following contributions: 1) It proposes a novel DPDC system (to the authors’ knowledge, this is the first time this model has been developed); 2) The dynamic model of the proposed DPDC system is developed; 3) Based on the dynamic model, the elemental motions of the DPDC system are obtained; 4) A closed-loop control algorithm using the partial feedback linearization technique is developed for the motion control of the DPDC system. 5) The dynamic model and the motion control strategy of the DPDC system are demonstrated using numerical simulation. This paper is organized as follows. In Section II, the dynamic model of the DPDC system is studied. In Section III, the system constraints and the pendulum trajectory are investigated. In Section IV, the elemental motions of the DPDC system are studied in detail. Simulation studies and results are presented in Section V and finally, conclusions are given in Section VI. II. SYSTEM MODELING A. System description m1 θ1 Pi

Fxi

l1 m2

Fzi

θ2 l2

yo

Zo

zo

.

xo

G (X, Y, Z)

Yo O Xo

Figure 1 The double-pendulum driven cart system Suppose that the DPDC is placed on a plane surface with inertial orthonormal basis (Xo, Yo, Zo) shown in

4

Figure 1. A local frame (xo, yo, zo) is assigned to the cart at its centre of mass G. The coordinates of G in (Xo, Yo, Zo) can be written as G = (X, Y, Z). Since only the plane motion of the cart is considered, the Z-coordinate of G is constant. The cart moves on the plane with four passive wheels, and two parallel pendulums are driven by two torque motors mounted on the pivots of the cart at P1 and P2, respectively. In particular, the following assumptions are defined before the modeling: 

The DPDC is a rigid system;



The weights of four wheels, two pendulum rods, and two torque motors are negligible;



Two pendulums only rotate on the xozo plane;



The cart has no lateral skidding.

B. System kinematics

YO

v2

yo ICR

.

vy2

yICR

vx2

.

W2 v

xo

vx1

VY

W1 .

v1

vy1

Y

vy

. xICR

vx

v3

Φ vy3

G

vx3

.W

3

vx4

W4 .

v4

vy4

O

X VX

XO

Figure 2 Velocities of the cart Figure 2 gives the diagram of DPDC on the XoYo plane. From Figure 2, it can be noted that the generalized velocity vector [VX VY]T (or X Y  T ) is related to the coordinate of the local velocity vector [vx vy]T as follows:

5

 X  cos     Y   sin 

 sin   v x  v x  v   R( ) v   cos    y   y

(1)

Differentiation with respect to time gives v x  v y  a x   X   R (  )  a      R( )   v y  v x Y   y

(2)

Let the instantaneous centre of rotation ICR=(x ICR,yICR) in xoyo axis. So, from Figure 3, the following relation can be obtained  x ICR   v y   y     ICR   v x  

(3)

The velocity for each wheel can be found v x1  v x 2  v x  c

(4.a)

v x3  v x 4  v x  c

(4.b)

v y 2  v y 3  ( x ICR  b)  v y  b

(4.c)

v y1  v y 4  ( x ICR  a)  v y  a

(4.d)

C. System dynamics Considering the interaction in Figure 1, the internal thrust generated at point Pi can be formulated as follows:  F   mi a x  mi lii cos  i  mi lii2 sin  i  Fi   xi     2  Fzi   mi g  mi lii sin  i  mi lii cos  i 

(5)

where i=1,2, mi is the ball’s mass, li the pendulum’s length, θi the angle between the pendulum and the zo axis, g the acceleration due to gravity. Using equation (5), the torque generated by the motor is given by  i  mi li a x cos  i  mi li2i  mi gli sin  i

(6)

6

Yo

yo W2

b a

P1

W1 . f1

.

T2

f2

.

xo

Fx1 2c

T1

d1 Mr

.

Mt

G

Fx2

d2

. P2

.W3 f3

T3

.W4 f4

T4

O

Xo

Figure 3 Free body diagram The active and resistive forces of the cart on the plane surface are shown in Figure 3 which gives the following relations mc a x  Fx1  Fx 2  f

(7.a)

mc a y  Fy1  Fy 2  T

(7.b)

I c  M t  M r

(7.c)

where mc is the cart’s mass, f the total friction force of the wheels along the xo axis, T the total friction force of the wheels along the yo axis, Ic the moment of inertia around the centre of mass G, Mt the active torque around G, Mr the resistant moment around G. Summing the forces applied on the cart along the zo axis gives the normal force N  mc g  Fz1  Fz 2

(8)

It is assumed that the vertical force Nj (j = 1…, 4) acts from the surface to the wheels which can be expressed as N1  N 4 

b N ab 2

(9.a)

N2  N3 

a N ab 2

(9.b)

7

So, the friction forces on the wheels can be written as f j   h N j sgn( v x j )

(10.a)

T j   v N j sgn( v y j )

(10.b)

To summarise, the total longitudinal resistive force is given by 4

f   f j  12  h N [sgn( v x1 )  sgn( v x 3 )]

(11)

j 1

and the total lateral friction force is given by 4

T  T j  j 1

v

a b

N [b sgn( v y1 )  a sgn( v y 2 )]

(12)

where μh and μv are coefficients of the longitudinal and lateral friction forces, respectively. Using equations (11) and (12), the resistive torque around G can be obtained as M r  a(T1  T4 )  b(T2  T3 )  c[( f 3  f 4 )  ( f1  f 2 )] 

 abv a b

N [sgn( v y1 )  sgn( v y 2 )]  12 c h N [sgn( v x 3 )  sgn( v x1 )]

(13)

and the active torque around G is M t  Fx 2 d 2  Fx1 d1

(14)

Putting equations (5), (11)-(13) in (7) gives 2

2

i 1

i 1

(mc  m1  m2 )a x   mi lii cos  i   mi lii2 sin  i

(15.a)

 12  h N [sgn( v x1 )  sgn( v x 3 )]  0 (mc  m1  m2 )a y 

v

a b

N[a sgn( v y1 )  b sgn( v y 2 )]  0

2

2

i 1

i 1

I c  a x  (1) i mi d i   (1) i mi li d i (i2 sin  i  i cos  i )

(15.b)

(15.c)

v 1  ab a b N [sgn( v y1 )  sgn( v y 2 )]  2 c h N [sgn( v x 3 )  sgn( v x1 )]  0

Transferring coordinates from the local frame (xo, yo, zo) to the inertial frame (Xo, Yo, Zo) by using (2) gives 2

MX  cos   mi l ii cos  i  h1 cos   h2 sin   0

(16.a)

i 1

2

MY  sin   mi l ii cos  i  h1 sin   h2 cos   0 i 1

(16.b)

8 2

2

i 1

i 1

( X cos   Y sin  ) (1) i mi d i   (1) i mi l i d ii cos  i  I c  h3  0 2

(16.c)

2

where M  mc   mi , h1   mi lii2 sin  i  12  h N[sgn( v x1 )  sgn( v x3 )] , h2  ab N[b sgn( v y1 )  a sgn( v y 2 )] , v

i 1

i 1

2

2

i 1

i 1

v i 1 h3   (1) i mi li d ii2 sin  i  ab a b N  (1) sgn( v yi )  2 c h N [sgn( v x 3 )  sgn( v x1 )]

Let q = [X Y θ1 θ2 Φ]T, combining equations (6) and (16), a compact form can be obtained as below D(1 ,  2 ,  )q  H (1 ,  2 , 1 , 2 ,  )  Bu

(17)

where M   0  2 i D( 1 ,  2 ,  )   cos   (1) mi d i i 1    m1l1 cos  1 cos   m 2 l 2 cos  2 cos 

0 M

 m1l1 cos  1 cos   m1l1 cos  1 sin 

 m 2 l 2 cos  2 cos   m 2 l 2 cos  2 sin 

sin   (1) i mi d i

m1l1 d 1 cos  1

 m 2 l 2 d 2 cos  2

 m1l1 cos  1 sin   m 2 l 2 cos  2 sin 

m1l12 0

0 m 2 l 22

2

i 1

 h1 cos   h2 sin   0  h sin   h cos   0 2  1 ,   B  0 H ( 1 ,  2 , 1 , 2 ,  )   h3    1   m1 gl1 sin  1  0   m 2 gl 2 sin  2 

0 0 0  0 1

0 0  , Ic    0 0 

and u   1   2 

III. MOTION STRATEGY A. Definitions We assume that the parallel pendulums only rotate within the upper half of the xozo plane shown in Figure 1. To explain the motion strategy of the system, the following definitions are given. Fast motion: rotating the parallel pendulums fast leads to a large internal thrust (|Fxi|>>|f|/2) along the xo axis; Slow motion: reversing the pendulums back to the initial position slowly (|Fxi|