ROBUST ADAPTIVE CONTROL OF MOBILE MANIPULATOR

In this paper a robust control is applied to a twowheeled mobile manipulator (WMM) to observe the dynamic behavior of the total system. To do so, the dynamic equation of the mobile manipulator is derived taking into account parametric uncertainties, external disturbances, and the dynamic interactions between the mobile platform and the manipulator; then, a robust controller is derived to compensate the uncertainty and disturbances solely based on the desired trajectory and sensory data of the joints and the mobile platform. Also, a combined system which composed of a computer and a multidropped PICbased controller is developed using USBCAN communication to meet the performance of demand of the whole system. What’s more, the simulation and experimental results are included to illustrate the performance of the robust control strategy.

ROBUST ADAPTIVE CONTROL OF MOBILE MANIPULATOR

Tan Lam Chung (1) , Sang Bong Kim (2)

(1) National Key Lab of Digital Control and System Engineering, VNU-HCM

(2) Pukyong National University, Korea

ABSTRACT: In this paper a robust control is applied to a two-wheeled mobile

manipulator (WMM) to observe the dynamic behavior of the total system To do so, the dynamic equation of the mobile manipulator is derived taking into account parametric uncertainties, external disturbances, and the dynamic interactions between the mobile platform and the manipulator; then, a robust controller is derived to compensate the uncertainty and disturbances solely based on the desired trajectory and sensory data of the joints and the mobile platform Also, a combined system which composed of a computer and a multi-dropped PIC-based controller is developed using USB-CAN communication to meet the performance of demand of the whole system What’s more, the simulation and experimental results are included to illustrate the performance of the robust control strategy.

Keywords: robust adaptive controller, mobile manipulator

1 INTRODUCTION

The design of intelligent, autonomous machines to perform tasks that are dull, repetitive,hazardous, or that require skill, strength, or dexterity beyond the capability of humans is theultimate goal of robotics research Examples of such tasks include manufacturing, excavation,construction, undersea, space, and planetary exploration, toxic waste cleanup, and roboticassisted surgery Robotics research is highly interdisciplinary requiring the integration ofcontrol theory with mechanics, electronics, artificial intelligence, communication and sensortechnology

A mobile manipulator is of a manipulator mounted on a moving platform Such thecombined system has become an attraction of the researchers throughout the world Thesesystems, in one sense, considered to be as human body, so they can be applicable in manypractical fields from industrial automation, public services to home entertainment

In literature, a two-wheeled mobile robot has been much attracted attention because of itsusefulness in many applications that need the mobility Fierro, 1995, developed a combinedkinematics and torque control law using backstepping approach and its asymptotic stability isguaranteed by Lyapunov theory which can be applied to the three basic nonholonomicnavigations: trajectory tracking, path following and point stabilization [2] Dong KyoungChwa et al., 2002, proposed a sliding mode controller for trajectory tracking of nonholonomicwheeled mobile robots presented in two-dimensional polar coordinates in the presence of theexternal disturbances [5]; T Fukao, 2000, proposed the integration of a kinematic adaptivecontroller and a torque controller for the dynamic model of a nonholonomic mobile robot [4]

On the other hand, many of the fundamental theory problems in motion control of robotmanipulators were solved At the early stage, the major position control technique is known to

be the computed torque control, or inverse dynamic control, which decouples each joint of therobot and linearizes it based on the estimated robot dynamic models; therefore, theperformance of position control is mainly dependent upon the accurate estimations of robotdynamics Spong and Vidyasaga [8] (1989) designed a controller based on the computedtorque control for manipulators The idea is to exactly compensate all of the coupling

nonlinearities in the Lagrangian dynamics in the first stage so that the second stagecompensator can be designed based on linear and decoupling plant Moreover, a number oftechniques may be used in the second stage, such as, the method of stable factorization wasapplied to the robust feedback linearization problem [9] (1985) Corless and Leitmann [10](1981) proposed a theory based on Lyapunov’s second method to guaranty stability ofuncertain system that can apply to the manipulators.

In this paper, a robust control based on the work of [11] was applied to two-wheeledmobile platform and a 6-dof manipulator taking into account parameter uncertainties andexternal disturbances In [11], the controller was only applied to a two-link manipulator, andthe platform is fixed To design the tracking controller, the posture errors of the mobileplatform and of the joints are defined, and the Lyapunov functions are defined for the two suchsubsystems and the whole system as well The robust controllers are extracted from thebounded conditions of the parameters, disturbances and the sensory data of the mobilemanipulator Also, the simulation and experimental results show the effectiveness of thesystem model and the designed controllers And this works was done in CIMEC Lab.,Pukyong National University, Pusan, Korea

2 DYNAMIC MODEL OF THE WMM

The model of the mobile manipulator is shown in Fig 1

First, consider a two-wheeled mobile platform which can move forward, and spin about itsgeometric center, as shown in Fig 2 The length between the wheels of the mobile platform is

band the radius of the wheels isr w {OXY} is the stationary coordinates system, or worldcoordinates system; {Pxy} is the coordinates system fixed to the mobile robot, and P is placed

in the middle of the driving wheel axis; C(x c,y c)is the center of mass of the mobile platformand placed in the x-axis at a distanced from P; the length of the mobile platform in thedirection perpendicular to the driving wheel axis is a and the width isL It is assumed that thecenter of mass Cand the origin of stationary coordinate Pare coincided The balance of themobile platform is maintained by a small castor whose effect we shall ignore

Fig 1 Model of the mobile manipulator

C P

L a

y c

d

a=550 b=260

r w =220 L=400 d=0

Fig 2 Mobile platform configuration

Second, the manipulator used in this application is of an articulated-type manipulator withtwo planar links in an elbow-like configuration: three rotational joints for three degrees offreedom They are controlled by dedicated DC motors Each joint is referred as the waist,shoulder and arm, respectively Also, the manipulator has a 3-dof end-effector function as roll,pitch and yaw; and a parallel gripper attached to the yaw

The length and the center of mass of each link are presented

as(L b1,Z b1),(L b2,Z b2),(L b3,Z b3), (L b4,Z b4),(L b5,Z b5), respectively The geometric modeland the coordinate composed for each link is shown in Fig 3

L b

L b4 =150 Lb5 =140

Link 2 Shoulder

Link 1 Waist

Z7

X1 Z0

The dynamics of the mobile manipulator subject to kinematics constraints is given in thefollowing form [11]:

d

v

T v a

v a

v

E

q A F

F q

q C C

C C q

q M

1 22

21

12 11 22

P

Fig 4 Kinematic analysis of tracking problem

The tracking errors of the mobile platform are given as follows [2]:

r

y y

x x e

e

e

e

100

0cossin

0sincos

r

e v

e v v e e e

10

2 2

1

0

cos 1 2

1 2

1

k

e e

sin ) cos

2

3 3

1

k

e e

v v

1 1 3

sin

cos

e k e v k

e k e v v

r r

r d

d

Wherek1, k2andk3are positive values

The Eq (7) becomes

3 2 2

3 2

k

ClearlyV10and the tracking errors ee1,e2,e3Tis bounded along the system’s solution

It is also assumed that not only the velocity of v r  0is constant with the orientation rbutalso the reference angular velocityris bounded and have its bounded derivative for allt.From Eqs (5), (6) and (9), it is shown that e and e are bounded, so thatV1, that is,

1

V isuniformly continuous SinceV1does not increase and converges to some constant value, byBarbalat’s lemma,V10ast 0 Ast, the limit of Eq (9) becomes

2 3 3

Eq (10) implies that e1 e3T  0ast

From Eq (5), the derivative of errore3is given

Since v r2e3has the limit equal to zero whent, the derivative of this term can bederived as follows:

2

2 2 3

2.2 Lyapunov function for the mobile platform

Consider the firstm-equation of Eq (1) as follows:

v v d T v a

v a

f C

HereM11S T M11S ,C S T C S S T M S

11 11

w w v

T

r b r b

r r E S

/ /

/ 1 / 1

It can be seen that f1, which consists of the gravitational and friction force vectorF1 andthe dynamics interaction with the manipulator(M12qa C12qa), and the disturbances on themobile platform (terrain disturbance force) needs to be compensated online

Assumption 2: The friction and gravity on the mobile platform are bounded

by F1(q v,qv)  0 1 q , where0 and1representing some positive constants

The velocity tracking error is defined as

M C

M11~ 11~  11  11  1 1 (18)Let us consider the following Lyapunov function

) (

~

1 1 11 11

2.3 Lyapunov function of the manipulator

Consider the last n-equations of Eq (1),

a d v

v a

a

a ad

~

~

0,

~

~

a a a ad a a

a

T a a

a

q k r k q q q

k

q

r

k k q

[

2 1

2 2 22 22

22

22 22

2

d a

a a

T

a

a T a a T

a

f kr M q C k M r

r M r r

2.4 Lyapunov function of the mobile manipulator

The Lyapunov function for the overall system, the mobile platform and the manipulator,can be defined and rearrange as follows:

~ (

2

1 ) (

~ (

~ ) (

~ ( 2

1 )

~ ( 2

1 )

~ ( )

1

0

22 12

11 0

22 12

11 0

22 12

12 11

0

22 12 12 11 0

V

V

r M r S M r M

V

r M r S M r S M S V

r M r r M S S M r S M S

V

r

S M M M M r

S

V

V

T T a

a T a T

T a T

a T a T

T a T

T

a T a a T T

T a T

a T

dt

d V V

) (

~

21 2

2 22 22

22

1 1 11 11 0

d f

kr M q k C k M

r

f C M V

V

T d a a a

T

d d d v

12

1 12 12

1

)

~ )(

(

) (

)

~ )(

(

)

~ ( )

~ (

) (

f q k r k M C r M

S

F q C q M S q k r k M C r M

S

F q k r q C q k r k r q

M

S

F q C q

M

S

f

a a a

T

ad ad T a a a

T

a a ad a

a a ad T

a a T

21 21

2 21 21 21

2 22 22

21 21

2 22 22

21 21

2

)

~ )(

( )

~ )(

(

) (

)

(

) (

) (

) (

f S

C S M S

M

f S C S M S

M

F q C q M S C S S

M

F q C q M q C q

M

f

d d

ad ad

ad ad

v v

T a d T v

1 M d C d f S T C k q a M k r ak q a

d d d

The nonlinear terms1and2are need to be identical online using robust control scheme

in the following section based on the work in [14]

3 ROBUST CONTROLLER DESIGN

First, consider the second term of the Eq (32) and using Properties (1)-(3) and Assumptions (1)-(2):

1 1

0 12

11 12

11

1 1

12 11

12 11

1 1

12

11 12

11

1 12

12 1

11 11

~

~

)(

~

)

~(

~

~

~)

~(

T

N s

a ad b s

d b a a ad b s d b v

T

d T T

a ad

T d

a a ad

T d T

v

T

d T T a ad T

d a

a ad

T d T

v

T

d

T a a a

T d

d v

T

q q

q k q C

q C

q k r k q M M

S F S q k q C S C

q k r k q M S M

S F S q k q C S

C q k r k q M S M

q k r k M q k C S f C M

~ ,

, )

~ ( ,

(

) (

, , , , ,

(

1

1 0 1 12 11 12 11

1

q q q k q q q

k r k q

C C M M

a ad d a a ad d

T

s N s

b s b b s b

2 3

2 21

22

21 22

2 2 21

22

21 22

2 2 21

22

21 22

2 21

21

21 2 22

22 22

~

)

~(

~

)

~(

)

~(

)(

)

~(

~)(

T

a

N d

s b a

ad b

d d b a

a ad b a

a

T

a

d d

a ad

d d a

a ad T

a a

T

a

d d

a ad

d d a

a ad T

a a

T

a

d

T a d

d

d a

a a

T

a

r r

q C

q q q C

S S M q r k q M r r

F S

C q q C

S S M q

r k q M r r

F S C q q C

S S M q r k q M r r

r S

C S

M

S M f q k M C kr M

~,

,)

~((

,,,

,,(

2

2 3 12 22 12 22

2

q q q

q k q S S q k r k q

C C M M

d a

ad d d a a ad

T

s b b b b T

22 

wherek pv0, k pa 0,k11 0,andk22  0are the controller gains;1and2are the robustdamping control vectors, respectively Then the tracking errors of the closed-loop system are

guaranteed to be globally uniformly ultimately bounded.

Substituting (36) and (37) into (32) yields

min

2 max 2

2

2 2 2

1

1 1 min

2

2 2 1

2 1 2

2

2 2 2

2

1

1 1 1

0

2 2

2 2 2

2

2 2 2

2 1

2 1 2

1

1 1 1

0

2

2 2 2

2

2 2 1

1 1 2

1

2 1

0

2 2

2 2

2 2 1 1

2 1

2 1

0

2 2

2 2 2 1

1

2 1 1 0

22

2

442

2

42

42

r k

k

k k k

r k k

k

V

k k

r k k k

k

V

k

r r

k k

k

V

r r

k k

V

r r

r k r r k k

k

V

V

a d

a d

a d

a a

d d

a a

d d

T a a

T a a

T a pa T

T T

T pv

wherekmin  mink1,k2andmaxmax1, 2 

In Eq (38), max is a bounded quantity; therefore, V decreases monotonically until thesolutions reach a compact set determined by the right-hand-side of Eq (32) The size of theresidual set can be decreased by increasingkmin According to the standard Lyapunov theoryand the extension of the LaSalle theory, this demonstrates that the control input Eqs (36) and(37) can guarantee global uniform ultimate boundedness of all tracking errors

Block diagram of robust controller is shown in Fig 5

Platform’s Robust Controller

Manipulator

Mobile Platform

1

v ) E ( 

pv

k

Kinematics Controller Coord.

+

Fig 5 Block diagram of robust controller

4 CONTROL SYSTEM DEVELOPMENT

The control system is based on the integration of computer and PIC-based microprocessor.The computer functions as high as high level control for image processing (not presented inthis paper) and control algorithm and the microprocessor, as low level controller for devicecontrol The configuration diagram of the overall control system is shown in Fig 6 In theconfiguration, there are 8 Servo-CAN modules are used to control all low-level devices: waistmotor, shoulder motor, arm motor, roll motor, pitch motor, and yaw motor for manipulator;and left-wheeled and right-wheeled motors for mobile platform The positions of themanipulator’s joints feedback to the computer via ADC module on USB-CAN The Servo-CAN module is based on PIC18F458 shown in Fig 8 The USB-CAN module is used tointerface between high and low levels: it transforms the serial data from computer to CANmessages shown in Fig 7 The USB-CAN interface is shown in Fig 9

For the operation, USB camera Logitech 4000 is used to capture the image stream intomemory with size of 320x240 at 30fps using QuickCam SDK The image is processed usingimage processing library OpenCV to extract the features from the image for the object’sposition detection The torque command is sent to the low level to control the mobile platformand the manipulator to perform a certain task The total control processes are programmed and

integrated into the interface IMR V.1 in Visual C++ shown in Fig 10 With this interface, themobile manipulator’s parameters can be set before the operation.

FT245BM USB Controller

PIC18F458 USB/CAN Controller TX-CAN RX-CAN

MCP2551 CAN Transceiver

93C64 EEPROM

PIC18F458 Servo CAN

PIC18F458 Servo CAN

PIC18F458 Servo CAN

PIC18F458 Servo CAN

PIC18F458 Servo CAN

PIC18F458 Servo CAN

PIC18F458 Servo CAN

Fig 6 Diagram of the control system

Fig 7 USB-CAN module

Fig 8 Servo CAN module

Fig 9 USB-CAN Interface

Fig 10 Mobile manipulator interface IMR V.1

5 SIMULATION RESULTS

To verify the effectiveness of the controller, the simulations have been done withcontroller (8),(36) and (37) using Visual C++6 (with matrix package) and Gnuplot 4 Thereference trajectory are planned for the total system: a cubic spline reference line for themobile platform shown in Fig 11 with the trajectory parameters in Fig 12 and sinusoidtrajectory for joint 1 to joint 6 with the frequency of 0 8f, 0 9f,f, 1 1f, 1 2f and

isx( 0 )  0 1m, y( 0 )  0 3m,( 0 )  45,x r( 0 )  0 3m, y r( 0 )  0 5m,r( 0 )  0, respectively; theinitial joint positions,q a( 0 )  (/ 6 ,/ 10 ,/ 18 , / 18 , / 10 , / 6 ).Sampling time is 10ms Thedisturbance is a random noise with the magnitude of 3, but it is not considered in thissimulation

Simulation results are given through Figs 13-29 The mobile platform‘s tracking errors aregiven in Fig 13 for full time (8s) and Fig 12 for the initial time (2s), respectively It can beseen that the platform errors go to zero after about 1 second The kinematics control input, thelinear and angular velocities at mobile platform center, is shown in Fig 15; the velocities ofthe left and right wheel, in Fig 16 It can be seen that the tracking velocity is in the vicinity of2m/s as desired, but it should be tuned for the acceptable value in the practical applications.The robust vector for the mobile platform1(i 1 6 ) in the controller Eq (36) is given in Fig

17, and the torque on the left and the right wheel, in Fig, 18 The mobile platform’s trackingposition with respect to the reference trajectory is shown in Fig 19 for the initial time (2s) and

in Fig 20 for full time (8s) As for the manipulator, the tracking positions of joint 1 to joint 6are shown in Figs 22-27 It can be seen that the overall tracking performance is acceptable,but they each are depend on the frequency of the reference trajectory The joint’s positionerrors and joint’s torques are shown in Figs 28 and 29, respectively

The experimental results are given through Figs 30-39 The reference trajectory for theexperiment is designed again to be satisfied the mechanical condition of the experimentalmobile manipulator The reference trajectory of the platform is given in Fig 30 The posture ofthe mobile platform can be calculated using dead-reckoning method via encoders, then theerrors e1, e2ande3can be derived shown in Figs 31- 33 The errors go to zero after about 3seconds The joint’s reference trajectory for the manipulator is designed as sinusoidaltrajectory with slower frequencies than those in the simulation, that is,

f f f

f

f, 0 9 , , 1 1 , 1 2

8

.

0 and1 3f (f  0 25Hz) The joint’s position errors are shown in Figs

34-39, respectively; they are satisfied the tracking performance in the manner of ordinaryapplications

Overall, the simulation and experimental results are reasonably same; it can be concludedthat the kinematics controller and the robust controller are all applicable for practical fields.The experimental test bed is shown in Fig 40

Ref linear velocity (m/s)

Ref angular velocity (rad/s)

Ref heading angle (rad)

Fig 12 Reference trajectory parameters for platform