Modeling and Dynamic Control of Autonomous Ground Mobile ...

University of Calgary

PRISM: University of Calgary's Digital Repository

Graduate Studies The Vault: Electronic Theses and Dissertations

2016

Modeling and Dynamic Control of Autonomous

Ground Mobile Manipulators

Mustafa, Mahmoud

Mustafa, M. (2016). Modeling and Dynamic Control of Autonomous Ground Mobile Manipulators

(Unpublished doctoral thesis). University of Calgary, Calgary, AB. doi:10.11575/PRISM/26936

http://hdl.handle.net/11023/2939

doctoral thesis

University of Calgary graduate students retain copyright ownership and moral rights for their

thesis. You may use this material in any way that is permitted by the Copyright Act or through

licensing that has been assigned to the document. For uses that are not allowable under

copyright legislation or licensing, you are required to seek permission.

Downloaded from PRISM: https://prism.ucalgary.ca

UNIVERSITY OF CALGARY

Modeling and Dynamic Control of Autonomous Ground Mobile Manipulators

by

Mahmoud Mustafa

A THESIS

SUBMITTED TO THE FACULTY OF GRADUATE STUDIES

IN PARTIAL FULFILMENT OF THE REQUIREMENTS FOR THE

DEGREE OF DOCTOR OF PHILOSOPHY

GRADUATE PROGRAM IN MECHANICAL AND MANUFACTURING

ENGINEERING

CALGARY, ALBERTA

April, 2016

© Mahmoud Mustafa 2016

ii

ABSTRACT

A Mobile Manipulators (MMs) in particular is an articulated robotic arm mounted

on a (space, ground, aerial, surface or underwater) mobile platform. The mobile platform

increases the size of the manipulator’s workspace and as a result the increased degree of

mobility enables better positioning of the manipulator in different configurations for

efficient task execution. However, it is a challenge to effectively control such systems

outside the lab and engineered environments such as those encountered in rough outdoor

environments and urban search and rescue applications where numerous uncertainties

exist. In these environments numerous model approximations have been used which cannot

be considered when working in real world outdoor conditions.

The main focus of this work is to develop needed mechanisms to enable the

deployment of MMs in unstructured terrains. For this a kinematic and dynamic model for

a generalized mobile manipulator without using approximations is developed. In addition,

such improved model is used for the control and robustness of mobile manipulator

controllers in the existence of dynamic uncertainties which has not been extensively

considered in prior work. Two new control algorithms are developed and shown to solve

the problem at hand with much better accuracy when compared to prior proposed solutions.

As a result of the proposed developed methodologies the proposed control system

architecture is shown to be applicable for the control of MMs performing a cooperative

task with other robots or humans. The control mechanism enables the MMs to execute

complex tasks when it is subject to dynamic uncertainties resulted from cooperation with

humans or other autonomous robots when working in unknown, dynamic, heterogeneous

outdoor rough terrains/environments. The approach is shown to be effective with the use

iii

of two different control solutions: i) robust sliding mode backstepping kinematic into

dynamics, and ii) stable robust adaptive Sliding mode backstepping CMAC Neural

Network control where both control systems use a Lyapunov function stability. Simulation

tests using a detailed SIMMECHANICS /SIMULINK model of the employed MMs are

presented to illustrate and demonstrate the performance of the developed control

mechanisms.

iv

ACKNOWLEDGEMENTS

I would like to thank my supervisor Dr. Alex Ramirez for his support over the years.

Without his insight and input, this thesis would not have been possible. I would like to

thank my friends in the AR2S-Lab at the University of Calgary: Krispin Davies, Graeme

Wilson, Diego LA Torre, Marshal Staples and Jie Wang for their support, advice,

encouragement and emotional support.

v

TABLE OF CONTENTS

ABSTRACT ........................................................................................................................ II ACKNOWLEDGEMENTS .............................................................................................. IV TABLE OF CONTENTS .................................................................................................... V LIST OF TABLES ........................................................................................................... VII LIST OF FIGURES AND ILLUSTRATIONS............................................................... VIII LIST OF SYMBOLS, ABBREVIATIONS AND NOMENCLATURE .......................... XI LIST OF SYMBOLS, ABBREVIATIONS AND NOMENCLATURE (CONTINUED)XII

CHAPTER ONE: INTRODUCTION ..................................................................................1 1.1 Motivation ..................................................................................................................1 1.2 Thesis outline .............................................................................................................7

CHAPTER TWO: LITERATURE SURVEY .....................................................................8 2.1 Introduction ................................................................................................................8 2.2 Control MMs in cooperation’s .................................................................................10 2.3 Related works ..........................................................................................................15 2.4 Summary ..................................................................................................................18

CHAPTER THREE: PROBLEM STATEMENT ..............................................................20 3.1 Problem statement addressed in this research thesis ...............................................20 3.2 Proposed solution .....................................................................................................21

3.2.1 Mathematical modeling (STEP 1): ..................................................................23 3.2.2 Simulation (STEP 2): ......................................................................................24 3.2.3 Control design of the (UGV) which will transport the robot arm (STEP 3): ..25 3.2.4 Control design of the mobile manipulator – UGV + robot arm (STEP 4): .....27

3.3 Proposed contributions ............................................................................................29

CHAPTER FOUR: MOBILE MANIPULATOR KINEMATIC MODELING ................31 4.1 Introduction ..............................................................................................................31 4.2 Mobile robot kinematic modeling ............................................................................32 4.3 Kinematic modeling of ana-bot mobile manipulator ...............................................37 4.4 Manipulator jacobian ...............................................................................................39 4.5 Manipulability analysis of the robot arm .................................................................40 4.6 Kinematic modeling of the mobile manipulator (mobile platform + robot arm) .....41 4.7. Summary .................................................................................................................44

CHAPTER FIVE: MOBILE MANIPULATOR DYNAMIC MODELING .....................45 5.1 Dynamic modeling of the nonholonomic UGV .......................................................45 5.2 Lagrange dynamic modeling of the nonholonomic UGV .......................................46 5.3 Dynamic model of the nonholonomic UGV ............................................................48 5.4 Dynamic modeling of the mobile manipulator (mobile platform + robot arm) .......51 5.5 Summary ..................................................................................................................62

CHAPTER SIX: SIMULATION FRAMEWORK ............................................................64

vi

6.1 Model of the mobile manipulator using simmechanics ...........................................64 6.2 Control architecture .................................................................................................67 6.3 Simulation and results ..............................................................................................70 6.4 Model of the mobile manipulator using simulink ....................................................74

CHAPTER SEVEN: TRAJECTORY TRACKING CONTROL OF MM ........................80 7.1 Introduction ..............................................................................................................80 7.2 Robust stable adaptive sliding mode backstepping neural network control of a UGV

80 7.3 Robust sliding mode CMAC control of a mobile manipulator ................................88 7.4 Summary ................................................................................................................94

CHAPTER EIGHT: SIMULATION RESULTS ...............................................................96 8.1 Introduction ..............................................................................................................96 8.2 Simulation framework and results of control of UGV ............................................96 8.3 Results for the UGV control strategy: .....................................................................97 8.4 Simulation framework and control results of the mobile manipulator ..................104 8.5: Results for the mobile manipulator control strategy .............................................106 8.6 Summary ..............................................................................................................113

CHAPTER NINE: CONCLUSIONS ...............................................................................115

BIBLIOGRAPHY ............................................................................................................117

vii

LIST OF TABLES

Table4.1: Kinematic wheeled mobile robot parameters. .............................................................. 34

Table 4.2: Kinematic ANA-Bot manipulator link parameters ...................................................... 38

Table 7.1: The parameters of CMAC for training the dynamics of UGV .................................... 85

Table 7.2: The parameters of CMAC for training the dynamics of MM. ..................................... 92

Table 8.1: Mobile Manipulator robot parameters. ........................................................................ 97

viii

LIST OF FIGURES AND ILLUSTRATIONS

Figure 2.1: Cooperative Transportation by Human Beings [32]. ................................................. 11

Figure 2.2: Complaint motion control of each robot [40]. ............................................................ 12

Figure 2.3: Centralized control structure [44]. .............................................................................. 13

Figure 2.4: Decentralized control structure [44]. .......................................................................... 14

Figure 2.5: MM’s Cooperative payload transport in 2D [53]. ...................................................... 16

Figure 2.6: Grasping of unknown objects by multiple mobile robots [55]. .................................. 17

Figure 2.7: Cooperation of MM handling rigid objects [56]. ....................................................... 18

Figure 3.1: The mathematical modeling flowchart of the system. ............................................... 24

Figure 3.2: Simulation flowchart of the system ............................................................................ 25

Figure 3.3: The control system flowchart of the mobile base (UGV). ......................................... 26

Figure 3.4: The control system flowchart of the mobile manipulator. ......................................... 28

Figure 4.1: ANA-Bot experimental MM platform. ..................................................................... 31

Figure 4.2: Wheeled mobile robot Manipulator model with frame {B}, and global frame {W}. ...................................................................................................................................... 33

Figure 6.1: Mobile Manipulator (ANA-Bot) SIMMECHANICS model...................................... 65

Figure 6.2: Two MMs cooperating SIMMECHANIC model. ...................................................... 66

Figure 6.3: Flowchart of the control algorithm. ............................................................................ 69

Figure 6.4: Joint position of the MM(i), i=1,2. ............................................................................. 70

Figure 6.5: Joint velocity of the MM (i), i=1, 2. ........................................................................... 71

Figure 6.6: Vehicle’s and arm’s linear velocities (i), i=1,2. ......................................................... 72

Figure 6.7: Changes of the arm’s manipulability (i), i=1, 2. ........................................................ 73

Figure 6.8: Graphical time lapsed simulation results of two MM handling object ...................... 74

Figure 6.9: MATLAB/Simulink model of the MMs .................................................................... 76

ix

Figure 6.10: MATLAB/SIMULINK model of the kinematics, jacobian and dynamics of the MMs ...................................................................................................................................... 77

Figure 6.11: Circular Trajectory of the mobile base in the robot`s x-y plane. ............................. 78

Figure 6.12: Arc Trajectory of the mobile base in the x-y plane. ................................................. 79

Figure 6.13: End-Effector`s Trajectory of the MM in the x-y-z plane. ........................................ 79

Figure 7.1: Control architecture of UGV ...................................................................................... 81

Figure 7.2: Control architecture of MMs ...................................................................................... 89

Figure 8.1: Simulation of the proposed control of UGV. ............................................................. 98

Figure 8.2: Simulation of the UGV in x-y plane (Circle reference trajectory). ............................ 99

Figure 8.3: The UGV actual output states time response vs. reference trajectories of UGV: (a) x, (b) y, and (c) q. ................................................................................................................ 100

Figure 8.4: The UGV actual output states error (a) ex, (b) ey, and (c) eq time response vs. reference trajectories of UGV. ............................................................................................ 101

Figure 8.5: Simulation of the UGV in x-y plane (Spiral reference trajectory). .......................... 102

Figure 8.6: The UGV actual output states time response vs. reference trajectories of UGV: (a) x, (b) y, and (c) q. ................................................................................................................ 103

Figure 8.7: The UGV output states error (a) ex, (b) ey, and (c) eq time response vs. reference trajectories of UGV. ............................................................................................................ 104

Figure 8.8: Simulation of the proposed control of MMs. ........................................................... 105

Figure 8.9: Simulation of the MM end-effector trajectory in x-y-z plane (Curve reference trajectory) ............................................................................................................................ 107

Figure 8.10: The MM actual output end-effector trajectory time response vs. reference trajectories of MM: (a) x (b) y and (c) z trajectories. ......................................................... 107

Figure 8.10: Continued. .............................................................................................................. 108

Figure 8.11: The MM tracking performance joint positions (a) Joint 1 and (b) Joint 2. ............ 108

Figure 8.11: Continued. .............................................................................................................. 109

Figure 8.12: The MM tracking performance joint velocities (a) Joint 1 and (b) Joint 2. ........... 109

Figure 8.13: Simulation of the MM end-effector trajectory in x-y plane (Top view). ............... 110

x

Figure 8.14: Simulation of the MM end-effector trajectory in x-z plane (Side view), ............... 110

Figure 8.15: Simulation of the MM end-effector trajectory in y-z plane (Front view). ............. 110

Figure 8.16: Simulation of the MM end-effector trajectory in x-y-z plane (Curve reference trajectory) ............................................................................................................................ 111

Figure 8.17: The MM output end-effector trajectory time response vs. reference trajectories of MM. ................................................................................................................................ 112

Figure 8.18: (a and b) The MM tracking performance Joint position ........................................ 113

Figure 8.19: (a and b) The MM tracking performance joint velocity ......................................... 114

xi

LIST OF SYMBOLS, ABBREVIATIONS AND NOMENCLATURE

Acronym Definition ANA-Bot Autonomous Navigation Assistant Robot CMAC Cerebellar Model Articulation Controller COG Center of Gravity DOF Degree of Freedom MLP Multi-Layer Perceptron MMs Mobile Manipulators UGVs Unmanned Ground Vehicles WMM Wheeled Mobile Manipulators WMR Wheeled Mobile Robot Roman Symbols Definition 𝐴𝐴(𝑞𝑞) Kinematic constraint matrix 𝑎𝑎1,𝑎𝑎2 𝑎𝑎𝑎𝑎𝑎𝑎 𝑎𝑎3 The length of link 1,2 and 3 𝑏𝑏 The distance between the left and the right wheel 𝑐𝑐 Gain matrix (sliding controller) 𝐶𝐶(𝑞𝑞, �̇�𝑞) Centripetal and Coriolis matrix 𝑎𝑎𝑚𝑚𝑚𝑚𝑚𝑚 Positive gain matrix �̂�𝑎𝑚𝑚𝑚𝑚𝑚𝑚 The robust term 𝑎𝑎𝑖𝑖 𝑖𝑖 = 1,2, . .𝑎𝑎, Offset between two links 𝑒𝑒, �̇�𝑒, �̈�𝑒 Position error, velocity error, acceleration error 𝐸𝐸(𝑞𝑞) Input transformation matrix 𝐹𝐹(𝑞𝑞, �̇�𝑞) Frictional vector 𝑓𝑓 Unknown dynamics function 𝑓𝑓 Approximate dynamic function 𝐺𝐺(𝑞𝑞) The gravitational vector 𝐼𝐼𝑏𝑏 The inertia of the platform 𝐼𝐼1, 𝐼𝐼2 𝑎𝑎𝑎𝑎𝑎𝑎 𝐼𝐼3 The moments of inertia of links 1,2 and 3 𝐾𝐾𝑚𝑚𝑖𝑖𝑚𝑚 Positive gain matrix 𝑘𝑘1,𝑘𝑘2 𝑎𝑎𝑎𝑎𝑎𝑎 𝑘𝑘3 UGV positive reference gain matrix

𝑙𝑙𝑏𝑏 Distance from the wheel to the WMR center of mass

𝑀𝑀(𝑞𝑞) Inertia matrix 𝑚𝑚𝑏𝑏 The mass of the UGV 𝑚𝑚1,𝑚𝑚2 𝑎𝑎𝑎𝑎𝑎𝑎 𝑚𝑚3 The mass of link 1,2 and 3 𝑞𝑞 UGV Heading angle 𝑞𝑞𝑑𝑑(𝑡𝑡) Desired positions �̇�𝑞𝑑𝑑(𝑡𝑡) Desired velocity �̈�𝑞𝑑𝑑(𝑡𝑡) Desired acceleration

xii

LIST OF SYMBOLS, ABBREVIATIONS AND NOMENCLATURE (CONTINUED)

�̇�𝑞𝑙𝑙 , �̇�𝑞𝑟𝑟 Angular velocity of the left and the right wheel 𝑞𝑞𝑟𝑟 , 𝑞𝑞𝑙𝑙 The rotation angle of the left and the right wheel 𝑅𝑅 Real number space 𝑅𝑅𝑚𝑚 Real n-vector space 𝑟𝑟 The wheel radius 𝑟𝑟1, 𝑟𝑟2 𝑎𝑎𝑎𝑎𝑎𝑎 𝑟𝑟3 The radius of link 1,2 and 3 𝑆𝑆(𝑞𝑞) Null space of constraint matrix 𝐴𝐴(𝑞𝑞) 𝑠𝑠 Sliding mode control 𝑡𝑡 Time 𝑇𝑇𝑖𝑖𝑖𝑖+1 𝑖𝑖 = 1,2, . .𝑎𝑎, Transformation matrix between robot joints 𝑡𝑡𝑟𝑟 Trace of matrix 𝑉𝑉(𝑥𝑥, 𝑡𝑡) Lyapunov function 𝑣𝑣𝑙𝑙 , 𝑣𝑣𝑟𝑟 Wheel linear velocity left and right respectively 𝑣𝑣𝑐𝑐𝑚𝑚𝑟𝑟𝑐𝑐 Reference UGV linear and angular velocity 𝑤𝑤 Weight matrices 𝑤𝑤� The weight matrices errors 𝑤𝑤� The weight matrices updates

𝑥𝑥,𝑦𝑦, 𝑞𝑞 Cartesian coordinates and orientation of the UGV

𝑥𝑥𝑏𝑏 ,𝑦𝑦𝑏𝑏 Absolute position of the center of mass of the UGV

�̇�𝑥𝑐𝑐𝑚𝑚𝑟𝑟𝑐𝑐, �̇�𝑦𝑐𝑐𝑚𝑚𝑟𝑟𝑐𝑐 Reference UGV velocity in x and y direction Greek Symbol Definition 𝛼𝛼𝑖𝑖 𝑖𝑖 = 1,2, . .𝑎𝑎, Twist angle 𝜀𝜀𝑐𝑐𝑚𝑚𝑟𝑟𝑐𝑐 Reference UGV position in x, y and q direction Γ The learning rate of the CMAC neural network 𝜆𝜆(𝑞𝑞) Lagrange constraint matrix 𝜏𝜏 Torque vector 𝜏𝜏𝑑𝑑 Disturbances 𝛾𝛾𝑖𝑖 Learning element of matrix Γ 𝜑𝜑 The set of shape function

1

CHAPTER ONE: INTRODUCTION

Unmanned Ground Vehicles (UGVs) have had in recent times a particular importance in

diverse military and civil/commercial applications (search and rescue, policing, etc.) and are

gaining popularity in other areas such as agriculture, window clearing, security and surveillance.

A mobile manipulators (MMs) as an entity within the class of UGVs, in particular, is a robotic arm

mounted on a (space, ground, aerial, surface or underwater) mobile platform (UGVs). The mobile

platform increases the size of the traditional manipulator’s workspace and as a result the increased

degree of manipulability of the arm enables better positioning of the manipulator in different

configurations for efficient task execution[1, 2]. MMs systems have been suggested for various

tasks including activities in hazardous environments (e.g., search & rescue), outdoor exploration

and space operations to name a few. The proposed work of this research work is targeted for

wheeled and tracked ground MMs but can be applicable (with suitable changes) to other types of

MMs including legged MMs. Thus, in what follows when MMs are mentioned we are specifically

refereeing to wheeled/tracked MMs. Despite their degree of mobility and potential uses the

complex physical structure, the highly coupled dynamics between the mobile platform and the

mounted robot arm, and the potential (common) nonholonomic dynamics of the mobile base are

some of the aspects that increases the difficulties of MM systems design and control.

1.1 Motivation

MMs received great interest in the last decade due to the ability of the MMs to manipulate

and execute different complex tasks such as space exploration, search, rescue and military

applications [3]. In the area of MMs this type of tasks presents an even greater challenge. When a

MM is required to deal with heavy or complex object manipulation tasks two or more MMs might

2

be required under such cases the accumulation of the individual capabilities provided by multiple

MMs working in cooperation is desirable and crucial for task implementation. There are different

reasons that can be considered for and when deploying multi-MM systems (swarms). Nevertheless,

one of the important motivations is that cooperative robotic systems can be used to enhance the

robotic system’s effectiveness specially when performing tasks in complex rough outdoor

environments. Cooperative robots are able to accomplish a greater number of tasks that are far

beyond the capabilities of individual robots. Ideally, to manipulate any large, heavy object, one

can incorporate much smaller and lighter robot modules that will fulfill the desired task. Such

modular and potentially flexible structure allows for the implementation of a “divide and conquer”

approach to take care of the complexities associated with manipulating heavy objects while

executing challenging tasks such as extracting a victim from within a collapsed building.

Cooperative robots are also have advantages from the viewpoint of redundancy and robustness.

System robustness can be enhanced by using a team of cooperative robots with respect to single

point failure. It is possible to reconfigure the team and each agent is assigned a new task if and

when needed. Redundancy is usually used in systems that require high fault tolerance and high

successful rate.

Cooperative robotics first came into the modern engineering researchers’ mind in the late

1980s with a special focus on multiple manipulators and multiple mobile robots. Interested readers

can refer to [3] and [4] for a detailed description of the diverse research areas in multi-robot

systems. Control of cooperative MMs have fascinated the consideration of several researchers e.g.

[5–9]. Enthusiasm in such systems comes from more prominent capacity of the mobile

manipulators in doing more confused and complex assignments which cannot be accomplished by

one system [10]. The utilizations of such systems vary from assembling and moving materials in

3

factories to missions in dangerous environments such as undersea and space environments. There

is an interest for robotic mechanical gathering and disassembly operations in space and subsea

applications, where the operations must be done without a vast arrangement of exceptional

hardware because of the unstructured and questionable environment [9].

In manufacturing, gathering and disassembly processes are separated into the subsequent two kinds

of duties: independent and cooperative. For the independent tasks, operator study the control of

the entire position and attitude of the robot, while for the cooperative tasks, the control of the

relative position, attitude, and interaction force among the robots end-effectors is deliberated. In

this case, two or more robots, for example it can be used to assemble the objects at hand, with

each object being held by one member of the robot team [9]. In order to achieve such tasks it is

necessary to employ hybrid control schemes to control the relative motion/force between the

objects and thus effectively carry out the task. The errand of mating two subassemblies is a general

sample of a helpful undertaking that likewise requires the control of the relative movement/power

of the end effectors. Other applications of MMs include exploration and saving tasks. In this case,

there is a need to find a solution to the cooperative system control duties between two mobile

manipulators handling a deformable (Flexible) object while potentially executing the task in

extreme adverse environmental conditions. Here two control approaches can be used: i)

centralized, and ii) decentralized. In the centralized implementation, a centralized controller

makes use of all agent states to issue the control signals, while in the decentralized case, each robot

module is equipped with single controllers which can only access its own states and the

corresponding control signal is generated locally. Manipulation is the most vital duty of robotic

arms and is a highly important aspect in MMs. Thus, the extension of this to multi-robot systems

naturally has been one of the important goals in cooperative robotics. There are many pertained

4

issues to be considered in the control process of MMs either independently or in a swarm system

such as:

• Synchronization of the subsystems,

• UGV + arm synchronization/cooperation,

• MM handling flexible elements/objects,

• MM control when interacting with and external entity (robot, human or machine),

• Control of the applied forces, and

• Motion planning.

At the high level, the multi robot system can be composed of a homogenous or a heterogeneous

set of robots having similar or different characteristics. Research subjects in this field that have

been particularly fine considered include multi-robot MMs path planning, traffic control,

formation generation, and formation keeping [4]. Though most of these subjects are now properly

well agreed, demonstration of the developed techniques in experimental cooperative robots

working in real complex spaces (rather than in simulation) has been very restricted.

Stable robust adaptive trajectory dynamic control of MMs is one of the key desirable

characteristics of robotic systems. Though much research effort has, and is currently, being

devoted to this area, limited consideration has been paid to physically interconnect robotic systems

which have a promise of many potential applications that mark them of specific interest, and a

need, for study. Object transport, search, rescue and manipulation of cooperative multi-robot or

robot-human systems, like swarm planetary rovers [9] and human-supervised of multiple mobile

robots [11], has been verified to be an viable approach to handle complex and heavy payloads in

5

unknown and rough terrain environments. The objective of our research is to develop a stable

robust control solution for nonholonomic MMs with inaccuracies and parameters uncertainties.

For this, we consider MMs operating in instructed environment with the aim to enable robots to

overcome any possible disturbance or dynamic uncertainties that might experience. The associated

dynamic uncertainties can and frequently occur when the MMs cooperates with either a human,

other robot(s) or a combination as well as from the unstructured rough terrain present in

outengineered environments. Due to the complexity of this task in this research, we will focus on

a specific type of MM consisting of a two-tracked wheeled differentially driven mobile base with

a three revolute joint manipulator mounted on top of the base. Herein, such system will

occasionally be referred as a two-wheeled differentially driven robot or WMM. The control of

such MMs show significant challenges due to the following aspects:

1.the nonholonomic constraints rising by reason of the use of tracks, the redundancy in actuation,

2. the interacting among the dynamics of the UGV and the manipulator in a MMs, and

3. the dynamic connection between diverse collaborating MMs.

It must be noted that other challenges exist, but not considered herein to eliminate keep the problem

bounded to a few (yet still complex) manageable aspects while still being able to enable such

systems to be deployed in real outdoor missions.

To alleviate some of the challenges the solution proposed in this thesis work the nonholonomic

constraints equation are integrated within the model equations without the essential to use

Lagrange multipliers. This should guarantee that the vehicle’s trajectories are nonholonomic and

that the velocity of each robotic unit can be resulting from the angular velocity of its wheels

through the no-sliding state. This is an approach that has been used by a number of researchers

with relative success [12, 13]. The reaction forces that the ground applies on the wheels of each

6

mobile platform comprising the team (Swarm) cannot be calculated analytically. As a result the

reaction forces cannot be resolved and they are distributed to the wheels of each platform in an

unknown way.

While the control of the mobile base(s) itself (themselves) is problematic, there still exist the

interaction between the arm and the object being manipulated. Traditionally MMs have been

controlled assuming the manipulated objects are rigid. Although the proposed solutions are

somewhat effective they had not considered the flexibility of the objects being manipulated. A

flexible object can be considered to have infinite degrees of freedom, each associated with a mass

particle in the body. Due to the flexible nature of the common manipulated object between two or

more MMs, the operational point coordinates of one manipulator are not determined by the

operational coordinates of other manipulators. As a result the flexible object responds to the

relative motion of the grasp points, howeverdoes not restrict it. Thus the combined system’s

degrees of freedom are not confined. On the contrary, if one also considers the flexible body’s

DOF, the combined DOF are increased to a theoretically infinite number. This is clearly not

required from a control approach. From the literature review of the relevant studies, it is well

known that collaborative manipulation of two MM handling a flexible object is a complex and

challenging task. The collaboration of two or MMs handling a flexible object has a high level of

intricacy when it comes to developing the corresponding dynamics of the system and formulating

suitable control schemes to accomplish the chosen motion of the (flexible) object.

This research addresses the control problem of multiple MMs cooperating handling a flexible

object while moving on an irregular (outdoor) terrain subject to (holonomic and nonholonomic)

kinematic restrictions in the existence of unknown parameters and instabilities (such as terrain

irregularity and flexible object dynamics).

7

1.2 Thesis outline

In this dissertation, Chapter 2 provides a literature review on the previous work done in the areas

of mobile manipulator control, application of the mobile manipulator and control of the

cooperation of mobile manipulators for handling objects. Chapter 3 introduce the problem

statement, the proposed solution and the thesis contribution. Chapter 4 introduces the kinematic

model and the Jacobian of the tracked wheeled nonholonomic mobile robot and the nonholonomic

redundant mobile manipulators considered in this thesis. Chapter 5 introduces the Lagrange

dynamic model of the tracked wheeled mobile robot and the nonholonomic redundant mobile

manipulators. In Chapter 6, a MATLAB/SIMULINK/SIMMECHANICS simulation for the

cooperation between two mobile manipulators handling a rigid object is provided with SIMULINK

simulation results for the whole kinematic/dynamic of the mobile manipulator. In Chapter 7 a

backstepping sliding mode control and backstepping sliding mode control CMAC Neural Network

of the tracked wheeled mobile robot was developed. Also introduces a new control algorithm of

the nonholonomic mobile manipulator and its corresponding MATLAB/Simulink simulation

results for the simulated nonholonomic MM. This chapter provides a detailed description of the

proposed robust backstepping Sliding CMAC Neural Network Controller and the corresponding

Lyapunov stability. The chapter also shows the effectiveness of the proposed control architecture

toward the control of MMs under dynamic uncertainties. In Chapter 8 simulation results of the

proposed control system. Chapter 9 concludes the thesis with a summary, discussion, conclusions

and future work.

8

CHAPTER TWO: LITERATURE SURVEY

2.1 Introduction

Mobile manipulators have lately received noticeable attention with an extensive variety of

applications, mainly due to the extended workspace provided to the arm as the rover moves and

thus extending the ability of the arm to reach targets that would occasionally be outside of the

manipulator’s reach. Due to such capabilities, mobile manipulators show several advantages over

the traditional fixed arm counterparts (traditional industrial robot arms) as well as over their

simpler unmanned ground vehicles (UGVs) having no manipulator(s): robot arms are more

maneuverable and transportable. So they offer an efficient application for spatial uses due to their

extended workspace that can be extended to the extent enabled by the used UGV capabilities.

Recently, there has been an massive interest in the autonomous path planning of mobile

manipulators due to the ability they show in replacing objects in a wide workspace by employing

their redundancy in terms of having two cooperating sub-systems (UGV + Manipulator) that can

be used for following the desired path/trajectory in numerous ways by controlling obstacle

avoidance [1], [14], [15]. Such robotic systems combine the ingenuity of the robot arm with the

extended workspace abilities of the mobile platform, which is especially suitable for applications

such as field and service robotic, which have need both manipulation and locomotion abilities.

Many researches have been published that address the problems of motion control of MMs (e.g.,

[16], [17] and [18}]). Examples of control approaches developed for MMs include: nonlinear

feedback control, computed torque control, decoupled task governor, null space dynamic control,

input-output decoupling control, neural network architectures, adaptive robust control, and sliding

mode [16–21].

9

A new methods of different terminal sliding styles was proposed by [22], the mobile manipulator

position based on state equality and inequality was investigated. A novel adaptive non-linear

limitation task space controller is designed by [23] for mobile manipulators with both kinematic

and dynamic uncertainties. This design has main advantages that it’s based only on the calculation

of the simplest term of the mobile manipulator equations. The using of a path planning algorithm

in linearization technique for the controlling of MMs was studied by [24], the mobile manipulator

studied was with two arms, in a point to point motion and in a trajectory tracking The advantage

of this method is that each sub-problem can be solved by the most appropriate technique. Authors

of [25] proposed a unified path following the MMs. In their method, there is no clear path

essentially for the mobile base to follow as well as for the end-effector. A dynamically steady

Jacobian inverse is designed for mobile manipulator [26], the effectiveness and consistency of the

new Jacobian had examined with computer simulations. In [27] high performance required for

MMs is achieved by developed a mobile flexible manipulator. A technique for discovery the

extreme allowable dynamic weight of a non-linear flexible link mobile manipulator was

investigated. Lyapunov theory was used to prove the stability of a closed loop system in a novel

control structure which makes the integration of linear controller and fuzzy network possible is

presented by [28]. To control nonholonomic mobile manipulator a simple technique is proposed

by [29], this technique ensure that the outputs of the subsystems to follow bounded auxiliary

signals. In [30], method of path planning for mobile manipulators was presented, where the

objective of the robot is to move along a trajectory carrying a tool. The desired reference track of

the mobile was not required to execute the simulations. A technique to control the mobile

manipulator was investigated in [31], the mobile manipulator contains a skid-steering mobile

platform where the virtual force concept was used.

10

Most applications of single mobile manipulator’s require the robot to dynamically interact with its

environment while providing a desired motion task such as pushing, pulling, cutting, and

excavating. The manipulator should be forced controlled in order to implement these tasks.

However, traditional force control schemes are generally designed to establish and maintain stable

contact with static environments. Numerous force control schemes including external force

control, hybrid position/force control, and impedance control have mainly been applied to mobile

manipulators in un-dynamic environments where the dynamic interactions are not considered.

Thus, there is a necessity to develop proper control mechanisms that will enable MMs be deployed

in situations where neglecting the dynamic interactions is not possible.

2.2 Control MMs in cooperation’s

During the past decades, research on mobile manipulators has mainly focus on the operation of

single MM having one and sometimes two (cooperative) robot arms. However, recently increasing

interests have been paid to cooperative operation of MMs among which two-mobile manipulator’s

based cooperation has received noticeable attention [32–37]. Human beings can work with each

other to perform many high-load tasks (see Fig 2.1) [32]. Cooperation between human beings is

known to have significant advantages in improving their capabilities. Likewise, it has been desired

to enable multiple robots to carry out tasks which cannot be done by a single robot through

cooperation, e.g. transporting a bulky object. Because of the high complexities associated with

robot cooperation, research on multi-robot based cooperative operation is a challenging field.

Although such area has attracted many researchers there is not yet a unified framework capable of

proper controlling such systems outside the lab environment other than tele-operated solutions.

11

Figure 2.1: Cooperative Transportation by Human Beings [32].

In [38], MMs manipulating a rigid object where the coordinate dynamic control was presented. A

single rigid object with motion constricted by environment was conducted by a cooperative

dynamic hybrid control technique for multiple robotic mechanisms [38]. The proposed technique

used the manipulator and object dynamics to control the motion of the object as well as to control

both constraint and internal forces when the object’s constraint motion applied. The authors in [38],

in addition to other robot assists, had achieved diverse milestones including: i) Effective (yet

incomplete) dynamic equations of the arm-object system which can be formed involving explicitly

the unified formulation of the force and kinematic constraint on various grasp types and the hyper

surface description of object motion constraint, and ii) Accurate laws for nonlinear state feedback

for the driving force of the joint that decouples and linearizes the system regarding to the object

motion, constraint force, and internal force. In such approaches dynamic hybrid control systems

with a servo compensator was successfully employed and extensively examined experimentally.

These techniques are useful for fine manipulation tasks using multiple robotic mechanisms in

engineered spaces but their solution lacks completeness in dynamic and unstructured environments.

The transportation of a single objet in coordination without adopting force/torque sensors was

reported in [39], where the decentralized control of multiple holonomic mobile robots was used.

In such methods impedance control has been conventionally used as a decentralized control

12

schemes of multiple UGVs transporting a single rigid object in coordination (Fig 2.3). In this

system, by assuming a defined impedance dynamics without using a force/torque sensor, robots

were controlled. The robots are able to transport an object in coordination using one of many typical

leader-follower control algorithms. T. Oosumi [40] illustrated a decentralized control of multiple

MMs handling a rigid object in coordination. The object’s motion path is given to one of the robots,

known as a leader, while the other robots, called the followers, estimate or predict the motion of the

leader through the motion of the interacting object. In turn the followers handle the object based on

the estimated reference. Each robot is independently controlled without noticeable communication

among them. Although different leader-follower type of robot control algorithms have been

examined, the work in [30] specifies the internal force applied to the body, while the object was

handled in coordination depended on robot dynamics.

Figure 2.2: Complaint motion control of each robot [40].

The proposed control algorithms reported in the literature have been experimentally applied to one

or two degree of freedom mobile robot (Fig 2.2) and designed for robots having velocity-controlled

actuators. Furthermore, such techniques have also been experimentally applied to two mobile

robots handling a rigid object, where each robot has one degree of freedom. Although the

13

experimental effects have illustrated the power of the proposed control schemes such algorithms

cannot be applied to nonholonomic robots. The leaderless and leader-following problems were

investigated by [41] for non-holonomic mobile manipulators. In the problem, the avoidance of the

collision and the formation control were considered under time delays. A entire body impedance

controller is presented for wheeled mobile humanoids the upper body was torque/controlled [42].

In order to enhance the cooperation among UGVs dispersed control of cooperating MMs

manipulating and transporting objects [43] as well as coordination of multiple MMs has been

discussed by O. Khatib and others [5], [44]. In Khatib’s research the vehicle/arm platform was

treated as a macro/mini structure where dynamic coordination strategy was used to control the

redundant system (Figures 2.3 and 2.4). For cooperative operations, Khatib introduced a new

devolved control structure depend on the enlarged object and a virtual linkage model that has been

shown to be better suited for mobile manipulator systems vs simple UGVs cooperation tasks.

Figure 2.3: Centralized control structure [44].

14

Figure 2.4: Decentralized control structure [44].

To reduce the complexities associated with MMs control where each vehicle needs to be fully

capable of making decisions cooperative control with loose handling has been extended to

cooperative control methods applying a leader- follower type multiple MMs [45]. In [46] two

novel solutions for the cooperative control for the manipulation of a load by a team of mobile

robots. The first one is a controller that tracking exact the load twist while, the second one is a

controller that guarantee the stabilization of the twist in the existence of the error. In [47] the

implementation of two MMs in a real-life manufacturing was studied, The two robots used actual

tools and workstations to perform their tasks. The motion problem of a MMs was separated into

three division, firstly, calculating the continuous changing and then estimating the connectivity

changes, and finally the universal motion was calculated [48]. Theoretical and experimental results

for flexible planning algorithm of a mobile manipulation was introduced by [49], the proposed

technique should improve the flexibility of the motion for the MMs. The transporting of a movable

object in environments with obstacles is investigated using a two-layered hierarchical architecture

by [50]. A virtual joint procedure is proposed to better applying quasi-velocities for the modeling

of wheeled MMs [51]. The mathematical modelling of tracking control of the uncertain affine

constrains mobile manipulator is investigated in [52]. In [35], an adaptive fuzzy control used to

control multiple MMs interacting with a surface that is non-rigid in the occurrence of disturbances.

15

2.3 Related works

Prior references neglect dynamic interactions among MMs thus making them inappropriate for

cooperation in a priori unknown environments. To cope with such limitations people at the

University at Buffalo developed cooperative payload carriage by robot swarms as illustrated in

(Fig.2.5). Such work included payload transport via semi-autonomous wheeled MM modules

combined to create a variable composite system, controlled as a collective, while diverse

uncertainties were regarded. Under such approach two types of the control arrangements were

developed i) the Leader-Follower approach and ii) a Decentralized Control approach. Virtual

prototypes were used to evaluate the performance of both methods to examine the control system

alternatives and testing various design in software ahead of actual implementation. The authors in

[53] also extended the control schemes to allow for the dynamic (force) of the system and

introduced a dynamic control routine that allows the manipulator's end-effector and the mobile

base or each MM to be individually controlled. In such approach a tracking/stabilization control

routine for the nonholonomic mobile base that permits it to follow moving paths besides the

stationary points of the handled object being manipulated was also introduced. The composite

multi DOF wheeled vehicle, composed by supporting a shared load at the combined end-effectors

was handled as an in-parallel mechanical system with expressed serial-chain arms. Although the

approach presented in [53] is good mathematical model of mechanical systems with closed

kinematic chains that requires the solution to a system of highly coupled differential equations.

16

Figure 2.5: MM’s Cooperative payload transport in 2D [53].

Although treating cooperative MMs while considering the dynamic interacting forces has been

proved to be effective to provide better control it was noticed that the numerical stiffness of the

systems calls for small time steps in the control effort order to have the required needed accuracy.

It is challenging to attain large multibody systems with multiple links and various kinematic loop

with interactive forward and real-time simulations. The structure brought by the formation

paradigm cut down the problem of simultaneous motion planning for the entire team to be a staged

motion planning problem. The principal benefits of such approach include reduce the control

problem of handling objects to determining the "best formation" to trace a desired path and

concurrently anticipating the team-optimal movement plans into individual movement plans for

all members of the team. This has been accomplished by coupling kinematic motion planning

techniques with creation/evaluation of effectiveness measures introduced for the overall formation.

The favored kinematic motion planning technique used is the Geometric Motion Planning Strategy

wherein each MM is adjusted with an induced vector field. Different relative position of each

mobile robot within the formation leads to various motion plans for individual MMs. Special

emphasis has been depend on establishing quantitative measures of formation quality using

appropriate metrics that permits subsequent optimization of the relative positions of mobile robots

17

within the formation. Such approaches, however, have been only developed on wheeled vehicles

and other types of MMs (e.g., tracked systems) have been unnoticed as they present greater

dynamical interactions (e.g., Terramechanic induces forces) which demand enhanced control

algorithms.

To cope with such limitations evaluation the effectiveness of two constructions for dispersed

movement planning of robot collectives inside an Artificial Potential Field (APF) framework has

been proposed [e.g, 35].

Figure 2.6: Grasping of unknown objects by multiple mobile robots [55].

The APF methods as well as other approaches sing AI methodologies have proven to be suitable

in handling diverse interactions among MMs. However, to the best of the author’s knowledge,

such methodologies have only been limited to certain common interactions. In an attempt to cope

with such limitations the mobile robot Helper [55], was developed at Tohoku University with the

focus on developing controls for smooth, natural, and safe human-robot cooperation which

involved collision models of the robot, its partner, and its surroundings. All of the objects in the

environment would be modeled in a simulation, and include information like size, shape, weight,

and velocity, so that the robot could move around without suddenly ramming into things. Objects

that it would manipulate directly would also be modeled with relevant grasping information so that

18

the robot would maintain proper grasp at all times. In addition to this “Object Model”, the robot

would model both itself and its human partner using a “Partner Model” in real-time. This would

eliminate potential self-collisions and model the human’s actions and estimate the object’s

trajectory. Combining these approaches would allow the robot to determine its course of action

during a cooperative task. The force-torque sensors in the robot’s wrists would detect when a

human was pushing/pulling, and react smoothly by taking into account its own joint range

limitations. In a similar attempt the robotics laboratory at Shimane University in Japan [56]

developed a Coordinated transfer control (Fig 2.7). When the carrier is huge and heavy over the

carrier ability of one cart, it is able to carry the given object in a coordinated fashion by using

multiple carts.

Figure 2.7: Cooperation of MM handling rigid objects [56].

2.4 Summary

From the discussions and approaches presented in this chapter for controlling the MMs it

is observed that the existing methods and approaches are diverse. However, the existing literature

only considers MM’s kinematic or dynamic control by the cooperation between the wheel and the

arm interacting in an engineered environment (e.g., office buildings or lab settings). Current

control methods and formulation do not provide solutions to enable MM’s with dynamic

19

uncertainties and unknown dynamic parameters on both indoor (structured) and outdoor

(unstructured) rough terrains.

Furthermore, current control mechanisms for MMs have neglected the dynamic

interactions encountered in outdoor environments which need to be considered for proper MMs

deployment in complex missions. The research on MM’s interactive control algorithm presented

in this thesis seeks to fill these voids whether the MM interacts with another MM, a human or an

unknown entity.

20

CHAPTER THREE: PROBLEM STATEMENT

3.1 Problem statement addressed in this research thesis

From the literature review, it is concluded that there are a number of problems that have to be

addressed in order to use and deploy mobile manipulators in the outside world. Such problems

include:

1) Cooperation mechanisms with flexible elements,

2) New sensing tools to better perceive the complexities found in unstructured environments

such as objects mechanical characteristics,

3) Limit the use of assumptions that have been traditionally being made in controlling MM’s

to enable them to deal with real world uncertainties where assumptions limit their operation

effectiveness,

4) Solve the closed chain mechanisms problem which results when MM’s cooperate and

interact with other robots or entities including humans.

5) Use and deal with the dynamic uncertainties which have been traditionally neglected when

controlling MM’s,

6) Solve the complexities associated when robots are faced with inevitable external

disturbances found when interacting in the real world,

7) Maneuver nonholonomic robots in confined spaces while managing their limited

movement characteristics.

8) Etc.

It must be noted that many other challenges exist, but not mentioned herein to keep the problem

bounded/focus to a few (yet still complex) manageable aspects while still being able to enable such

21

systems to be deployed in real outdoor missions. Due to the large scope of problems (refer to above

list) in the area or MM’s, in this thesis the focus is on Problems 5 and 6. The reason why such

aspects were considered and no the other ones is due to the fact that providing a solution for these

two aspects can provide a stepping stone what will lead to help resolve the rest of other aspects.

Thus, the specific problem to be addressed in this thesis is described as follow:

Design and implement a controller capable of tracking a reference

trajectory in outdoor 3D space for nonholonomic mobile manipulators in

the presence of parametric uncertainties and external disturbances.

The following section provides a brief outline of the proposed solutions that has been

taken to solve this problem in terms of control aspects.

3.2 Proposed solution

The objective of this work is to design and implementation a controller for a nonholonomic MM

to enable its end effector (the tip of its robot arm) to track a predefined trajectory in space in the

presence of parameters inaccuracy, uncertainties and outdoor disturbances. The goal is for the

system (UGV + robot arm comprising the given MM system) to cooperate as needed to guarantee

the end tool to meet the desired path in space. Such cooperation will need to deal with obstacle

avoidance, terrain complexities, arm singularity problems and other aspects such as arm’s

mobility.

The proposed control system is based on an adaptive CMAC (Cerebellar Model Articulation

Controller - a type of Neural Network based on a model of the mammalian cerebellum) neural

22

network to enhance the performance of a backstepping sliding mode control tracking trajectory

performance.

The work will be performed in a series of four steps or phases where each provides the stepping

stone and information needed by the subsequent steps.

STEP 1) First, a dynamic model will be obtained where no assumptions are used in expressing

as close as possible to the real system with all its complexities.

STEP 2) Second, simulate the state space control equation of the system in SIMMECHANICS,

Matlab and Simulink environment. The purpose of this step is to verify the

mathematical model of the system with all complexities and constraints is describing

the real system. The simulation is verified by the logic of the expected motion of the

system with some inputs.

STEP 3) Third, the CMAC is developed to identify the potentially unknown unstructured system

dynamics. A CMAC architecture was selected due to its ability to approximate high

nonlinearity systems, the mapping and training operations are extremely fast and the

algorithms are easy to implement which make it really suitable to real time adaptive

control systems. Here we will generate the required data of the control inputs and

optimize the corresponding parameter values of the CMAC architecture followed by

its implementation with the online MM controller. By using the high learning ability

of artificial intelligence neural networks, the assumption here is that the CMAC will be

able to perform control adaptively while coping with different dynamics uncertainties.

STEP 4) Fourth, a backstepping control design and implementation will be developed and an

adaptive backstepping kinematics into dynamics sliding-mode control will be used.

23

Steps 3 and 4 employ the well know Lyapunov stability theory, which will be used to guarantee

the stability of the system check and guarantee that the boundedness of the tracking is insured. For

this the stability of the whole control system, via the boundedness of the CMAC weight estimation

updates and errors will be used.

Each of the above mentioned sequential steps satisfies a specific objective which is summarized

as follows:

3.2.1 Mathematical modeling (STEP 1):

1) Develop a mathematical model of a MM system in Cartesian space without using any

approximations to enable better control of the targeted system in real world unstructured

environments (Kinematic and Dynamic).

2) Derive the Jacobian and analyze the manipulability of the robot arm.

3) Test the dynamic properties of the mathematical modeling

4) Eliminate the Lagrange multipliers from the dynamic systems,

5) Obtain the reduced order model and put it in the form of a state space control.

A flow chart of the activities to be performed in STEP # 1 is shown in Figure (3.1).

The process illustrated in Figure 3.1 shows the stages of the mathematical modeling of the

MM. The stages start by acquiring data about the configuration of the robot (via sensors) and

derive the independent kinematics for both the wheel and the arm. Subsequently we combine

both models to find the full kinematics for the MM. The next step is to derive the dynamics for

the MM and eliminate the Lagrange multiplier. Finally the stage concludes by obtaining the

state space control equation for the MM. This result will be used in STEP 2, 3 and 4.

24

Figure 3.1: The mathematical modeling flowchart of the system. 3.2.2 Simulation (STEP 2):

1. Simulate the mobile robot dynamics and kinematics in Simulink,

2. Simulate the mobile manipulator in Simulink and test the simulation based on

understanding the logic of motion of MMs,

3. Simulate and control of the MM in SIMMECHANICS based on the coordination between

the locomotion and manipulation,

4. Simulate and control two MMs handling rigid objects in SIMMECHANICS.

25


The goal of this phase is to verify the mathematical model in Matlab/Simulink environment. The

process start by simulate the dynamics and kinematics model of the system and follow the logic

of the motion to test the simulation. Next step is to model in SIMMECHANICS the MM in 3D

dimension by coordination between manipulation and locomotion. Finally simulate the

cooperation between two MMs handling rigid object in SIMMECHANICS environment.

Figure 3.2: Simulation flowchart of the system

3.2.3 Control design of the (UGV) which will transport the robot arm (STEP 3):

1. Develop a trajectory planning algorithm of the mobile robot,

2. Develop a backstepping kinematic control of the mobile robot,

3. Develop a sliding mode control for the UGV,

26

4. Derive the corresponding control law based on a Lyapunov candidate function and

sliding mode control,

5. Approximate the dynamics of the system via a CMAC neural network,

6. Check the stability and the robustness of the system in a formal manner.


Figure 3.3: The control system flowchart of the mobile base (UGV).

27

The process illustrated in Figure 3.3 is described as follow. The control flow chart of the UGV

starting by specified a predefined trajectory to the backstepping controller with a reference

information about the UGV. The next process to send the output of the error dynamics of the

UGV to sliding mode control which include robust term and a Lyapunov candidate function to

guarantee the stability. Finally the unknown dynamics of the UGV is approximated by using

neural network CMAC.

3.2.4 Control design of the mobile manipulator – UGV + robot arm (STEP 4):

1. Develop a combined trajectory planning of the MM system as a whole to enable the

cooperation of the UGV and the arm to achieve a given combined trajectory in space,

2. Implement the developed backstepping kinematic control (STEP 3) of the mobile robot,

3. Implement the sliding mode control (STEP 3),

4. Derived the combined control law based on a Lyapunov candidate function and sliding

mode control,

5. Approximate the dynamics of the MM via a CMAC neural network,

6. Check the stability and the robustness of the system,

7. Implement the pseudo inverse jacobian to handle the redundancy from the mobile

manipulator.

The process illustrated in Figure 3.4 is described as follow. The control flow chart of the MM

starting by specified a predefined trajectory of the end-effector of the MM. The next process

to send the output of the error dynamics of the MM to sliding mode control which include

robust term and a Lyapunov candidate function to guarantee the stability. Finally the unknown

dynamics of the MM is approximated by using neural network CMAC.

28

Figure 3.4: The control system flowchart of the mobile manipulator.

It must be noted that the four flow charts shown in Figures 3.1 to 3.4 are interconnected sub-

systems that will be used to form a complete control system for MM’s deployed in unstructured

and dynamic outdoor rough terrains (to be described in Chapter 7).

29

3.3 Proposed contributions

BY following the sequence of steps outlined in this chapter this thesis will achieve a simple yet

effective control strategy/algorithm with the corresponding software to enable mobile manipulator

systems to interact with the environment, perform tasks in complex terrains, and enable the arm(s)

to follow a predefined trajectory in existence of dynamic uncertainties and unknown parameters

of the robot. The proposed system is envisioned to display safe, easy to use, reliable and intelligent

features to enable the next generation of MM’s be taken out of the lab and into performing real

missions in the complexity of real environments. The proposed system will have a number of

possible benefits to society and be used in a number of industrial as well as non-industrial missions.

The envisioned contributions of this thesis are listed below:

1. Develop a mathematical framework to model MM’s in Cartesian space without using any

approximations with the goal to enable better control of such systems in real world

unstructured environments.

2. Develop a controller for cooperating MM’s that takes into the account the associated

(possible coupled) kinematic constraints between the MM’s to provide increased

adaptability for the coupled movement in unknown dynamic environments when unknown

dynamics due to parameter uncertainties and disturbances are considered. Under this

solution a goal will be to prove the stability of the system.

3. Develop a new neural network Cerebellar Model Articulation Controller (CMAC) control

system for nonholonomic redundant mobile manipulators with applications to MM’s. One

of the associated contributions is for this control system to combine the end-effector

trajectory of the mobile manipulator and the trajectory of a mobile platform (UGV) in a

30

single control system (something that has been attempted before but a complete solution

has not yet been found).

4. Use a redundancy resolution scheme and inverse kinematics to deal with the singularities

and the robot arm joints limits. This will include using the manipulability and Pseudo

inverse Jacobian in control architectures in the context of MM’s. The purpose of such

approach is to avoid online inverse kinematic calculations and the associated singularity

problems that have been problematic in in the control of MM’s. For this a joint space

dynamic system will be derived and a similar analysis for the control of MM cooperating

in swarm systems (2 or more MM’S) will also be carried out.

5. Develop a robust control solution for MM’s under dynamic uncertainties. The proposed

solution will include the nonholonomic velocity constraint and the motion of the MM so

that a truly MM’s is realized. This has not been considered widely and thus, a solution to

this aspect will be a contribution by itself extending the analysis of MM’s systems to

include a complete system’s dynamic model in joint space.

6. Develop a robust adaptive backstepping sliding mode controller using the neural network

CMAC approximation of the robot dynamics which will highly improve the traditional

backstepping controller tracking trajectory performance.

The following chapters (4, 5, 6 and 7) describe the four research steps or phases in more detail.

31

CHAPTER FOUR: MOBILE MANIPULATOR KINEMATIC MODELING

4.1 Introduction

Control and design of a mobile robot requires full system comprehension and a suitable

mathematical representation of the robot. In this chapter the kinematic modeling for ANA-Bot, a

Mobile Manipulator available in our robotics laboratory at the University of Calgary (Fig 1), is

presented.

Figure 4.1: ANA-Bot experimental MM platform. The kinematics modeling of the robot without considering the forces effect is provided in

the following pages leaving the dynamic aspects (Forces and torques) to the next chapter. First the

mathematical model of the UGV and its nonholonomic constraints is described. Then the

manipulator is added and all necessary kinematic relationships between the UGV and the arm are

formulated. Note that in contrast to traditional mobile robot modeling the motion of the UGV and

the MM is considered to occur in the three dimension space (as opposed to in the 2½D). At the

end of this chapter, the constraint and the null space matrices, as well as the Jacobian, important

aspects in this work, are assembled.

32

4.2 Mobile robot kinematic modeling

Kinematics is the branch of mechanics that researches the movement of material bodies without

including their masses, inertia and the forces torques that deliver the movement. The mobile

platform used in this research is composed of a rigid UGV platform and a differential track drive

system considered as rigid wheels as the track rollers do not deform. To move forward/backward,

both motors drive the tracks in the same direction and speed. Alike to other traditional differential

drive systems, in track drive schemes, a robot can make turns at different radios of curvature

including making turns on the spot. The track drive system as certain advantages over traditional

wheeled robots such as climbing up slopes, steps, or going over obstacles and holes. Thus, this

system was selected in this work to enable testing the proposed system on outdoor rought terrains.

With the exception of the (dynamic) terramecanic interations between track and wheel vehicles,

the kinematic equations of track drive systems are the same as the differential drive. Thus in what

follows we do not consider the dynamics, which are included in Chapter 5.

Figure 4.1 shows the frames of reference used to derive the UGV’s kinematic modelwhere the

Robot’s frame of reference, {B}, is positioned at the center of the robot and considered aligned

with the tracks’ (wheels’) axis of rotattion. Such system has three positional degrees of freedom

yet the robot can instantly move only in two directions (back/forth and left/right rotation) due to

the existence of a velocity level constraint. That is the nonholonomic constraint (i.e., no lateral

motion is possible). This constraint prevents the mobile robot to have any lateral motion thus

making it impossible to have a velocity parallel to the direction of the tracks’ axles. Figure 4.1

shows the UGV + robot arm with its local frame of reference, denoted by {B}, and positioned

within a global frame {W} which represents the actual position of the robots in the real world. The

following conditions, partially discussed in Chapter 1, are used during the mathematical modeling:

33

• The maximum velocity of the vehicle is smaller than 5 km/h (neglect high speed navigation

thus preventing high inertias in the system).

• Longitudinal slide of the vehicle is neglected (not possible unless the vehicle slides and

slips which is not considered in this work).

• The contact between the wheel and the horizontal plane (ground) is reduced to a single

point (distributed track-surface contact with the ground is abandoned).

• Movement of the vehicle is assumed to be in 2½ dimensional space (where 2½D is used

herein to denote rough terrain).

• The rotational plane of each wheel stays perpendicular to the ground (thus the robot can

pitch and roll without affecting its kinematics).

Figure 4.2: Wheeled mobile robot Manipulator model with frame {B}, and global frame

{W}.

34

Consider a differentially-driven nonholonomic UGV moving in a plane where a body stable frame

{B}, is attached to it at the center of the axle. Let the forward direction of travel be as shown in

Figure 4.1, along the robot’s xb axis. Let 𝑞𝑞 be the orientation of frame {B} with respect to the

world coordinate frame {W}.

The following table (Table 4.1) provides the robot parameters for ANA-Bot (Fig. 4.1 and 4.2)

used in the kinematic modeling:

Table4.1: Kinematic wheeled mobile robot parameters.

Parameter Description

𝑏𝑏 the distance between the left and the right wheel

𝑟𝑟 the wheel radius

𝑙𝑙𝑏𝑏 distance from the wheel to the UGV center of mass

�̇�𝑞𝑙𝑙 , �̇�𝑞𝑟𝑟 angular velocity of the left and the right wheel

𝑞𝑞 UGV absolute rotation angle

𝑣𝑣𝑙𝑙 , 𝑣𝑣𝑟𝑟 wheel linear velocity left and right respectively

𝑥𝑥𝑏𝑏 ,𝑦𝑦𝑏𝑏 absolute position of the center of mass

To generalize the model we consider a UGV with {n} generalized coordinate’s {q} subject to 𝑚𝑚

mechanical constraints:

𝐴𝐴(𝑞𝑞)�̇�𝑞 = 0 (4.1)

where: 𝐴𝐴𝐴𝐴𝑅𝑅𝑚𝑚×𝑚𝑚 is a full rank matrix.

The configuration (pose) of the UGV is described by the following vector as a function of the

wheel angles (𝑞𝑞𝑟𝑟 , 𝑞𝑞𝑙𝑙)

35

𝑞𝑞 = 𝑓𝑓(𝑞𝑞𝑟𝑟 𝑞𝑞𝑙𝑙),𝜀𝜀 = [𝑥𝑥𝑏𝑏 𝑦𝑦𝑏𝑏 𝑞𝑞]𝑇𝑇 (4.2)

Next we use the corresponding rotational matrix, R (q), which is used to describe the robot’s

heading w.r.t. the inertial frame of reference {W}:

𝑅𝑅(𝑞𝑞) = �𝑐𝑐𝑐𝑐𝑠𝑠 𝑞𝑞 −𝑠𝑠𝑖𝑖𝑎𝑎 𝑞𝑞 0𝑠𝑠𝑖𝑖𝑎𝑎 𝑞𝑞 𝑐𝑐𝑐𝑐𝑠𝑠 𝑞𝑞 0

0 0 1� (4.3)

Due to the fact that the UGV has no steering wheel the rotating motion is driven differentially so

the sequence of any movement of the UGV can only be controlled, without violating the

nonholonomic slip constraint.

The right and left linear wheels velocity of the UGV can be found using Eq. (4.4) while the

vehicle’s total linear velocity is found using Eq. (4.5).

𝑣𝑣𝑙𝑙 = 𝑟𝑟 ∗ �̇�𝑞𝑙𝑙

𝑣𝑣𝑟𝑟 = 𝑟𝑟 ∗ �̇�𝑞𝑟𝑟 (4.4)

𝑣𝑣 =𝑣𝑣𝑙𝑙 + 𝑣𝑣𝑟𝑟

2=𝑟𝑟(�̇�𝑞𝑙𝑙 + �̇�𝑞𝑟𝑟)

2 (4.5)

The velocity and the angular rotation of the mobile platform is determined by a set of differential

equations in the subsequent form:

��̇�𝑥𝐵𝐵�̇�𝑦𝐵𝐵� = �cos 𝑞𝑞 − sin 𝑞𝑞

sin 𝑞𝑞 cos 𝑞𝑞 � �𝑟𝑟2

𝑟𝑟2

−𝑎𝑎. 𝑟𝑟 𝑏𝑏⁄ 𝑎𝑎. 𝑟𝑟 𝑏𝑏⁄� ��̇�𝑞𝑙𝑙�̇�𝑞𝑟𝑟

� (4.6)

36

�̇�𝑞 = −𝑟𝑟𝑏𝑏�̇�𝑞𝑙𝑙 +

𝑟𝑟𝑏𝑏�̇�𝑞𝑟𝑟 (4.7)

As a result, the kinematics model of the UGV’s velocity 𝑣𝑣 and the angular velocity �̇�𝑞 can be

represented in a matrix form as:

�𝑣𝑣�̇�𝑞� = �

𝑟𝑟2

𝑟𝑟2

−𝑎𝑎. 𝑟𝑟 𝑏𝑏⁄ 𝑎𝑎. 𝑟𝑟 𝑏𝑏⁄� ��̇�𝑞𝑟𝑟�̇�𝑞𝑙𝑙

� (4.8)

The UGV includes nonholonomic constraints that need to be included in its model. A

nonholonomic constraint is defined to be a restriction that contains time derivatives of position

coordinates of a framework that is not integrable [57]. To model this aspect we first characterize

a holonomic constraint as any limitation which can be mathematically denoted as:

𝑓𝑓(𝑞𝑞, 𝑡𝑡) = 0 (4.10)

where 𝑞𝑞 = �𝑞𝑞1,𝑞𝑞2, … … . 𝑞𝑞3�𝑇𝑇 is the vector of the robot’s position in its own coordinates (frame of

reference). To check the nonholonomic constraint mathematically, it is assumed that the

nonholonomic constraint of the UGV to be in the following form:

𝑓𝑓(𝑞𝑞, �̇�𝑞, 𝑡𝑡) = 0 (4.11)

If Eq. (4.11) cannot be integrated to the form of Eq. (4.10) this indicates that the UGV has

nonholonomic constraints and thus has some limitations in its mobility.

With respect to control, if the UGV has a number of actuators fewer than its total degree of freedom

(DOF) then the UGV has nonholonomic constraints. ANA-Bot has only two actuators while the

entire set of DOF of the system is 3. As a result this UGV systems has two nonholonomic and one

holonomic constraints. As a result the mobile robot’s motion is given as a nonholonomic

mechanical system where the following three kinematics constraints exist:

37

�̇�𝑥𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 − �̇�𝑦𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 = 0 (4.12)

�̇�𝑥 𝑐𝑐𝑐𝑐𝑠𝑠 𝑞𝑞 + �̇�𝑦 sin 𝑞𝑞 = 𝑟𝑟 ∗ �̇�𝑞𝑟𝑟 − 𝑏𝑏�̇�𝑞 (4.13)

�̇�𝑥 𝑐𝑐𝑐𝑐𝑠𝑠 𝑞𝑞 + �̇�𝑦 sin 𝑞𝑞 = 𝑟𝑟 ∗ �̇�𝑞𝑙𝑙 + 𝑏𝑏�̇�𝑞 (4.13)

The constraints can be written in a matrix form, where 𝐴𝐴 matrix is defined as:

𝐴𝐴 = �𝑠𝑠𝑖𝑖𝑎𝑎 𝑞𝑞 −𝑐𝑐𝑐𝑐𝑠𝑠 𝑞𝑞 0 0 0𝑐𝑐𝑐𝑐𝑠𝑠 𝑞𝑞 𝑠𝑠𝑖𝑖𝑎𝑎 𝑞𝑞 𝑏𝑏 − 𝑟𝑟 0𝑐𝑐𝑐𝑐𝑠𝑠 𝑞𝑞 𝑠𝑠𝑖𝑖𝑎𝑎 𝑞𝑞 − 𝑏𝑏 0 −𝑟𝑟

� (4.14)

In this kinematic model the angular velocities of the two wheels of the mobile robot are

independently controlled. If 𝑤𝑤 (the angular velocity of the wheels) and 𝑣𝑣 (the UGV’s linear

velocity) are provided then one can calculate 𝑞𝑞𝑟𝑟∙ and 𝑞𝑞𝑙𝑙∙ (the angular velocities of the right and left

wheels) as:

𝑞𝑞�̇�𝑟 =𝑣𝑣 + �𝑙𝑙 2� �

𝑟𝑟�̇�𝑞

𝑞𝑞�̇�𝑙 =𝑣𝑣 − �𝑙𝑙 2� �

𝑟𝑟�̇�𝑞

(4.15)

The previous set of equations (Eqs. 4.5, 4.6, 4.8, 4.14 and 4.15) represents the mathematical

kinematic model of ANA-Bot and are used in the control development.

4.3 Kinematic modeling of ana-bot mobile manipulator

Following the kinematic model of the UGV one can now proceed to generate the kinematic

model for the MM (UGV + robot arm). Based on the coordinate transformation idea, the coordinate

fixed on the UGV platform can be transformed to the base coordinate frame of the manipulator

(robot arm) by two translational and one rotation matrix. The frames of reference of the

manipulator under consideration are shown in (Figure 4.2). The transformation matrix between the

manipulator’s frames (having 3 DOF) is obtained using the Denavit_Hartenberg method. In

38

general form, transformation matrix between two adjacent manipulator links is derived as shown

in Eq. (4.16) using the parameters in Table 4.2:

𝑇𝑇𝑖𝑖𝑖𝑖+1 = �

𝑐𝑐𝑞𝑞𝑖𝑖 −𝑠𝑠𝑞𝑞𝑖𝑖𝑐𝑐𝛼𝛼𝑖𝑖𝑠𝑠𝑞𝑞𝑖𝑖 𝑐𝑐𝑞𝑞𝑖𝑖𝑐𝑐𝛼𝛼𝑖𝑖

𝑠𝑠𝑞𝑞𝑖𝑖𝑠𝑠𝛼𝛼𝑖𝑖 𝑎𝑎𝑖𝑖𝑐𝑐𝑞𝑞𝑖𝑖−𝑐𝑐𝑞𝑞𝑖𝑖𝑠𝑠𝛼𝛼𝑖𝑖 𝑎𝑎𝑖𝑖𝑠𝑠𝑞𝑞𝑖𝑖

0 𝑠𝑠𝛼𝛼𝑖𝑖0 0

𝑐𝑐𝛼𝛼𝑖𝑖 𝑎𝑎𝑖𝑖0 1

� (4.16)

Table 4.2: Kinematic ANA-Bot manipulator link parameters

𝒊𝒊 𝜶𝜶 𝒂𝒂 𝒅𝒅 𝒒𝒒

1 π 0 𝑎𝑎1 𝑞𝑞1

2 0 𝑎𝑎2 0 𝑞𝑞2

3 0 𝑎𝑎3 0 𝑞𝑞3

Therefore the last link transformation matrix with respect to the base of the manipulator is

achieved as:

𝑇𝑇03 = 𝑇𝑇01 ∗ 𝑇𝑇12 ∗ 𝑇𝑇23

𝑇𝑇01 = �

cos 𝑞𝑞1sin 𝑞𝑞1

00

0010

sin 𝑞𝑞1−cos 𝑞𝑞1

00

00𝑎𝑎11�

𝑇𝑇12 = �


00

−sin 𝑞𝑞2cos 𝑞𝑞2

00

0010

𝑎𝑎2cos𝑞𝑞2𝑎𝑎2 sin 𝑞𝑞2

01

�

𝑇𝑇23 = �


00

−sin 𝑞𝑞3cos 𝑞𝑞3

00

0010

𝑎𝑎3cos𝑞𝑞3𝑎𝑎3 sin 𝑞𝑞3

01

�

(4.17)

As a result; the forward kinematics equation of the manipulator is obtained as:

39

𝑇𝑇03 = �

𝑎𝑎𝑚𝑚 𝑐𝑐𝑚𝑚𝑎𝑎𝑦𝑦 𝑐𝑐𝑦𝑦

𝑎𝑎𝑚𝑚 𝑝𝑝𝑚𝑚 𝑎𝑎𝑦𝑦 𝑝𝑝𝑦𝑦

𝑎𝑎𝑧𝑧 𝑐𝑐𝑧𝑧0 0

𝑎𝑎𝑧𝑧 𝑝𝑝𝑧𝑧0 1

� (4.18)

The forward kinematic for the robot arm is thus:

𝑇𝑇03 = 𝑇𝑇01𝑇𝑇12𝑇𝑇23 = �

𝑐𝑐1 𝑐𝑐23𝑠𝑠1 𝑐𝑐23𝑠𝑠230

−𝑐𝑐1 𝑠𝑠23−𝑠𝑠1 𝑠𝑠23𝑐𝑐230

𝑠𝑠1 −𝑐𝑐1

00

𝑐𝑐1(𝑎𝑎2 𝑐𝑐2 + 𝑎𝑎3 𝑐𝑐3)𝑠𝑠1(𝑎𝑎2 𝑐𝑐2 + 𝑎𝑎3 𝑐𝑐3)𝑎𝑎1 + 𝑎𝑎2𝑠𝑠2 +𝑎𝑎3 𝑠𝑠23

1

� (4.19)

The position of the end effector of the manipulator in coordinate frame {𝑒𝑒} (see Fig. 4.2) with

respect to the base frame of the robot arm base (point {0} in Fig. 4.2) is shown in Equation (4.20):

𝑝𝑝𝑥𝑥0𝑒𝑒 = cos 𝑞𝑞1 (𝑎𝑎2cos 𝑞𝑞2 + 𝑎𝑎3cos𝑞𝑞3),

𝑝𝑝𝑦𝑦0𝑒𝑒 = sin 𝑞𝑞1 (𝑎𝑎2cos𝑞𝑞2 + 𝑎𝑎3cos 𝑞𝑞3)

𝑝𝑝𝑝𝑝0𝑒𝑒 = 𝑎𝑎1 + 𝑎𝑎2 sin 𝑞𝑞2 +𝑎𝑎3sin(𝑞𝑞2 + 𝑞𝑞3)

(4.20)

where:

𝑐𝑐𝑖𝑖 = 𝑐𝑐𝑐𝑐𝑠𝑠 (𝑞𝑞𝑖𝑖) for 𝑖𝑖 = 1, . . ,5 , 𝑠𝑠𝑖𝑖 = 𝑠𝑠𝑖𝑖𝑎𝑎 (𝑞𝑞𝑖𝑖) for 𝑖𝑖 = 1, . . ,5 , 𝑠𝑠23 = sin (𝑞𝑞2 + 𝑞𝑞3), and 𝑐𝑐23 =

cos (𝑞𝑞2 + 𝑞𝑞3).

4.4 Manipulator jacobian

As part of the manipulator’s model a Jacobian matrix is needed throughout the modeling

process and subsequent control development. Herein it will be used to define the dynamic

relationship within the MM. Each element in the Jacobian is the derivative of an equivalent

kinematic equation with respect to one of the variables. This means that the first kinematic equation

must represent movements along the 𝑥𝑥 axis, which in our particular case would be 𝑝𝑝𝑚𝑚. In other

words, 𝑝𝑝𝑚𝑚 expresses the motion of the hand frame of reference along the 𝑥𝑥 axis. To achieve this

40

the corresponding elements of the end effector, 𝑝𝑝𝑚𝑚,𝑝𝑝𝑦𝑦and 𝑝𝑝𝑧𝑧 , are differentiated to get the

corresponding Jacobian components.

The last column of the forward kinematic equation of the robot, Eq. (4.19), representing

the end effector position are differentiated w.r.t. 𝑞𝑞1:𝑞𝑞3. As a result is the following functions are

obtained:

��̇�𝑝𝑚𝑚�̇�𝑝𝑦𝑦�̇�𝑝𝑧𝑧� = 𝐽𝐽 �

�̇�𝑞1�̇�𝑞2�̇�𝑞3� (4.21)

��̇�𝑝𝑚𝑚�̇�𝑝𝑦𝑦�̇�𝑝𝑧𝑧� = �

−𝑠𝑠1 ∗ (𝑎𝑎2𝑐𝑐2 + 𝑎𝑎3𝑐𝑐23) −𝑐𝑐1 ∗ (𝑎𝑎2𝑠𝑠2 − 𝑎𝑎3𝑠𝑠23) −𝑎𝑎3𝑐𝑐1𝑠𝑠23𝑐𝑐1 ∗ (𝑎𝑎2𝑐𝑐2 + 𝑎𝑎3𝑐𝑐23) −𝑠𝑠1 ∗ (𝑎𝑎2𝑠𝑠2 − 𝑎𝑎3𝑠𝑠23) −𝑎𝑎3𝑠𝑠1𝑠𝑠23

0 𝑎𝑎2𝑐𝑐2 + 𝑎𝑎3𝑐𝑐23 𝑎𝑎3𝑐𝑐23� ��̇�𝑞1�̇�𝑞2�̇�𝑞3�

(4.22)

��̇�𝑞1�̇�𝑞2�̇�𝑞3� = �

−𝑠𝑠1 ∗ (𝑎𝑎2𝑐𝑐2 + 𝑎𝑎3𝑐𝑐23) −𝑐𝑐1 ∗ (𝑎𝑎2𝑠𝑠2 − 𝑎𝑎3𝑠𝑠23) −𝑎𝑎3𝑐𝑐1𝑠𝑠23𝑐𝑐1 ∗ (𝑎𝑎2𝑐𝑐2 + 𝑎𝑎3𝑐𝑐23) −𝑠𝑠1 ∗ (𝑎𝑎2𝑠𝑠2 − 𝑎𝑎3𝑠𝑠23) −𝑎𝑎3𝑠𝑠1𝑠𝑠23

0 𝑎𝑎2𝑐𝑐2 + 𝑎𝑎3𝑐𝑐23 𝑎𝑎3𝑐𝑐23�

−1

��̇�𝑝𝑚𝑚�̇�𝑝𝑦𝑦�̇�𝑝𝑧𝑧� (4.23)

With the Jacobian the total power required at the end effector to achieve the task at hand can be

determined. The corresponding total power (force) transported by the joint actuators must be

computed via Eqn. (4.24):

�𝐹𝐹𝑚𝑚𝐹𝐹𝑦𝑦𝐹𝐹𝑧𝑧� = [𝐽𝐽𝑇𝑇]−1 �

𝜏𝜏1𝜏𝜏2𝜏𝜏3� (4.24)

4.5 Manipulability analysis of the robot arm

In order to enable an effective cooperation between the UGV and the robot arm comprising the

MM in this thesis we employ the arm’s manipulability concept. Manipulability is the distance

between the end effector of the manipulator arm and the singularities points [58]. For effective

41

cooperation we attempt to maximize the manipulability of the arm by effective UGV motions.

That is, we suggest that by moving the UGV within the available space one can enable the arm to

perform any task (within the realm of possibilities) without being limited by the singularity points

of the arm. The arm’s joint velocity equation shows the relation between the velocities in Cartesian

space coordinates and the velocity of the robotic joints and how one can move the arm. To check

the manipulability of the robot arm the determinant of the Jacobian is derived. And used to

compute how effective the arm can achieve the task:

𝑀𝑀𝑎𝑎𝑎𝑎𝑖𝑖𝑝𝑝𝑀𝑀𝑙𝑙𝑎𝑎𝑏𝑏𝑖𝑖𝑙𝑙𝑖𝑖𝑡𝑡𝑦𝑦 = det (𝐽𝐽) = 𝑎𝑎3𝑎𝑎2(𝑎𝑎2𝑐𝑐2 + 𝑎𝑎3𝑐𝑐23)(−𝑐𝑐3)

The manipulability equals zero when(𝑎𝑎2𝑐𝑐2 + 𝑎𝑎3𝑐𝑐23) = 0. In such cases the manipulator’s end-

effector is stretched and placed in line with joint 1 (see Fig. 4.2). In such instances there is no

potential motion of the end-effector in the direction normal to the plane of Link 2 and Link 3.

During the UGV and arm cooperation we aim to move the UGV in such a way that the arm is

always far from such cases while still following any desired path in 3D (as it will be described in

Section 6.2, Chapter 6).

4.6 Kinematic modeling of the mobile manipulator (mobile platform + robot arm)

With the kinematic model of the UGV (Section 4.2), the arm (Sections 4.3 and 4.4) and the

associated mechanisms (Section 4.5) to be used (e.g., manipulability) we can proceed to combine

them into a complete MM model. When combining the models of the UGV platform and the robotic

arm result in a kinematic and dynamic model MM that it is complex, strongly coupled with a high

degree of dynamic interactions [57]. Despite the fact that nowadays a number of MMs are equipped

with different wheel types and wheel arrangements (e.g., tricycle, differential, care like platform,

tracked, and Mecanum wheels) intended to extend the vehicle’s maneuverability very little work

42

has been done on using complete models with the goal of control. Here we propose to use a rather

complete (to the best of our abilities) MM model for control in outdoor rough terrains.

Considering the MM of Figure 4.1 and its corresponding kinematic models obtained in the above

sections the following kinematic equation is obtained:.

As before, using the following three coordinate frames: i) 𝑋𝑋𝑤𝑤𝑌𝑌𝑤𝑤𝑍𝑍𝑤𝑤 - the world coordinate frame,

ii) 𝑥𝑥𝑝𝑝𝑦𝑦𝑝𝑝𝑝𝑝𝑝𝑝 - the center of the mobile robot coordinate frame, and iii) 𝑥𝑥𝑒𝑒𝑦𝑦𝑒𝑒𝑝𝑝𝑒𝑒 - the end-effector of

the MMs coordinate frame the end effector’s position and orientation with respect to the world

coordinate frame 𝑋𝑋𝑤𝑤𝑌𝑌𝑤𝑤𝑍𝑍𝑤𝑤 is given by Equation 4.25.

𝑇𝑇𝑒𝑒𝑤𝑤 = 𝑇𝑇𝑏𝑏𝑊𝑊𝑇𝑇𝑒𝑒𝑏𝑏 (4.25)

where:

𝑇𝑇𝑏𝑏𝑊𝑊 is the homogenous transformation matrix from {𝑊𝑊} to {𝐵𝐵} frame, and 𝑇𝑇𝑒𝑒𝑏𝑏 is the homogenous

transformation matrix from {𝐵𝐵} to {𝑒𝑒} frame.

Now the UGV kinematic model of Eq. (4.7) is to be integrated with the robot arm manipulator

kinematic model of Eq. (4.20). The result is Eqn. (4.26) which represents the position of the MM’s

end effector in 3D space ( 𝑥𝑥𝑊𝑊𝑒𝑒 , 𝑦𝑦𝑊𝑊𝑒𝑒 , 𝑝𝑝𝑊𝑊𝑒𝑒 ) as it is moved on top of the UGV during its motion on the

given terrain.

𝑥𝑥𝑊𝑊𝑒𝑒 = 𝑥𝑥0 + 𝑙𝑙𝑏𝑏 cos 𝑞𝑞 + 𝑎𝑎2 cos 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) + 𝑎𝑎3 cos(𝑞𝑞2 + 𝑞𝑞3) cos(𝑞𝑞 + 𝑞𝑞1)

𝑦𝑦𝑊𝑊𝑒𝑒 = 𝑦𝑦0 + 𝑙𝑙𝑏𝑏 sin 𝑞𝑞 + 𝑎𝑎2 cos 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) + 𝑎𝑎3 cos(𝑞𝑞2 + 𝑞𝑞3) sin(𝑞𝑞 + 𝑞𝑞1)

𝑝𝑝𝑊𝑊𝑒𝑒 = 𝑝𝑝 + 𝑎𝑎1 + 𝑎𝑎2 sin 𝑞𝑞2 + 𝑎𝑎3 sin(𝑞𝑞2 + 𝑞𝑞3)

(4.26)

where 𝑝𝑝 is the height of the UGV platform.

43

Now in order to find the Jacobian of the MM the partial derivative of 𝑥𝑥𝑊𝑊𝑒𝑒 ,𝑦𝑦𝑊𝑊𝑒𝑒 and 𝑝𝑝𝑊𝑊𝑒𝑒 with respect

to 𝑥𝑥𝑏𝑏: 𝑞𝑞3 are computed. The result are the following eighteen functions:

𝐽𝐽11 = 1

𝐽𝐽12 = 0

𝐽𝐽13 = −𝑙𝑙𝑏𝑏 sin 𝑞𝑞 − 𝑎𝑎2 cos 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) − 𝑎𝑎3 cos(𝑞𝑞2 + 𝑞𝑞3) sin(𝑞𝑞 + 𝑞𝑞1)

𝐽𝐽14 = − sin(𝑞𝑞 + 𝑞𝑞1) (𝑎𝑎2 cos 𝑞𝑞2 + 𝑎𝑎3 cos(𝑞𝑞2 + 𝑞𝑞3))

𝐽𝐽15 = − cos(𝑞𝑞 + 𝑞𝑞1) (𝑎𝑎2 sin 𝑞𝑞2 + 𝑎𝑎3 sin(𝑞𝑞2 + 𝑞𝑞3))

𝐽𝐽16 = −𝑎𝑎3 cos(𝑞𝑞 + 𝑞𝑞1) sin(𝑞𝑞2 + 𝑞𝑞3))

𝐽𝐽21 = 0

𝐽𝐽22 = 1

𝐽𝐽23 = 𝑙𝑙𝑏𝑏 cos 𝑞𝑞 + 𝑎𝑎2 cos 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) + 𝑎𝑎3 cos(𝑞𝑞2 + 𝑞𝑞3) cos(𝑞𝑞 + 𝑞𝑞1)

𝐽𝐽24 = cos(𝑞𝑞 + 𝑞𝑞1) (𝑎𝑎2 cos 𝑞𝑞2 + 𝑎𝑎3 cos(𝑞𝑞2 + 𝑞𝑞3))

𝐽𝐽25 = − sin(𝑞𝑞 + 𝑞𝑞1) (𝑎𝑎2 sin 𝑞𝑞2 + 𝑎𝑎3 sin(𝑞𝑞2 + 𝑞𝑞3))

𝐽𝐽26 = −𝑎𝑎3 sin(𝑞𝑞 + 𝑞𝑞1) sin(𝑞𝑞2 + 𝑞𝑞3))

𝐽𝐽31 = 𝐽𝐽32 = 𝐽𝐽33 = 𝐽𝐽34 = 0

𝐽𝐽32 = 0

𝐽𝐽35 = 𝑎𝑎2 cos𝑞𝑞2 + 𝑎𝑎3 cos(𝑞𝑞2 + 𝑞𝑞3)

𝐽𝐽36 = 𝑎𝑎3 cos(𝑞𝑞2 + 𝑞𝑞3))

Which can be written in matrix form representing the linear velocity Jacobian:

𝐽𝐽𝑣𝑣𝑚𝑚𝑚𝑚 = �𝐽𝐽11 𝐽𝐽12 𝐽𝐽13 𝐽𝐽14 𝐽𝐽15 𝐽𝐽16𝐽𝐽21 𝐽𝐽22 𝐽𝐽23 𝐽𝐽24 𝐽𝐽25 𝐽𝐽26𝐽𝐽31 𝐽𝐽32 𝐽𝐽33 𝐽𝐽34 𝐽𝐽35 𝐽𝐽36

� (4.27)

This matrix will be employed in the control effort of the MM (Chapter 7).

44

4.7. Summary

In this chapter, a complete kinematic model of the UGV and the nonholonomic MM was presented.

The 3D Jacobian and other elements of the mobile platform (UGV), the robot arm, and the MM

system were derived. The obtained models included all necessary constraints including the

nonholonomic properties. The derived equations represent a complete 3D dimension kinematic

model not used in past (only 2D models have been used). Thus, I believe the equations are more

representative of the kinematics of the robot which are envisioned to provide better control. The

results of the modeling approach presented here can be easily extended to other MMs with reduced

or increased complexities (e.g., greater DOF in the robot arm). In the following chapter the

dynamic equations of the MM will be derived which will include all dynamic constraints without

any linearization of any aspects of the MM.

45

CHAPTER FIVE: MOBILE MANIPULATOR DYNAMIC MODELING

The mathematical modeling of the dynamics of the nonholonomic UGV mobile robot and the MM

systems deal with the derivation of the equations of motion which describe the whole energy of

the system [57]. This can be achieved by using (combining) the following two methods.

• The Newton-Euler technique equation: the strategy for using this method is to manage the

coupling relationship of the forces acting on the MM’s joints and translation of its links. It

must be noted that the use of this technique is not simple to use in systems with numerous

joints/links it is estimated that will provide the means to a better model and thus make

possible improved control of MMs.

• The Lagrange mathematical technique: this is an energy equilibrium equation and as a

result it is more appropriate for studying the connections of movement in a given system.

In this chapter the Lagrange will be utilized to acquire the associated elements describing

the complete MM in a simple, yet complete, manner.

5.1 Dynamic modeling of the nonholonomic UGV

The Lagrange dynamic methodology of a multilink robot has the following form.

𝑎𝑎𝑎𝑎𝑡𝑡�𝜕𝜕𝜕𝜕𝜕𝜕�̇�𝑞𝑖𝑖

� −𝜕𝜕𝜕𝜕𝜕𝜕𝑞𝑞𝑖𝑖

= 𝜏𝜏 𝑖𝑖 = 1,2, …𝑎𝑎 (5.1)

where:

𝑞𝑞𝑖𝑖 is the degree of freedom of the generalized coordinates, and 𝜏𝜏 is the external forces applied to

the robot joints actuators. The Lagrange, L (Eqn. 5.1), is defined as:

𝜕𝜕 = 𝑇𝑇 − 𝑃𝑃 (5.2)

46

where {𝑇𝑇} and {𝑃𝑃} are the kinetic and potential energies of the robots (UGV or robot arm).

The kinetic energy of a robot is equal to:

𝑇𝑇 =12𝑚𝑚�̇�𝑣𝑇𝑇�̇�𝑣 +

12�̇�𝑞𝑇𝑇𝐼𝐼�̇�𝑞 (5.3)

where 𝑚𝑚 is the mass of the corresponding link, 𝐼𝐼 is the inertia of the corresponding robot link, �̇�𝑣

is the linear velocity of the center of the link, and �̇�𝑞 is the angular velocity of the link of interest.

In what follows the dynamic equations of the UGV (ANA-Bot) are derived followed by the

dynamic equations of the MM or interest.

5.2 Lagrange dynamic modeling of the nonholonomic UGV

The Lagrange mathematical model of the nonholonomic UGV has the following form:

𝑎𝑎𝑎𝑎𝑡𝑡�𝜕𝜕𝜕𝜕𝜕𝜕�̇�𝑞𝑖𝑖

� −𝜕𝜕𝜕𝜕𝜕𝜕𝑞𝑞𝑖𝑖

= 𝐸𝐸(𝑞𝑞)𝜏𝜏 − 𝐴𝐴𝑇𝑇(𝑞𝑞)𝜆𝜆 𝑖𝑖 = 1,2, …𝑎𝑎 (5.4)

where 𝐴𝐴𝑇𝑇(𝑞𝑞) is the 𝑚𝑚 ∗ 𝑎𝑎 nonholonomic constraint matrix dimention:

𝐴𝐴(𝑞𝑞)�̇�𝑞 = 0 (5.5)

and 𝜆𝜆 is the Lagrange multiplier. This model prompts to a traditional compact form:

𝑀𝑀(𝑞𝑞)�̈�𝑞 + 𝐶𝐶(𝑞𝑞, �̇�𝑞) + 𝐹𝐹(𝑞𝑞, 𝑞𝑞̇ ) + 𝐺𝐺(𝑞𝑞) + 𝜏𝜏𝑑𝑑 = 𝐸𝐸(𝑞𝑞)𝜏𝜏 − 𝐴𝐴𝑇𝑇(𝑞𝑞)𝜆𝜆 (5.6)

where 𝐸𝐸(𝑞𝑞) is a nonsingular transformation matrix, 𝐶𝐶(𝑞𝑞, �̇�𝑞) is the centripetal and Coriolis matrix,

𝐹𝐹(𝑞𝑞, �̇�𝑞) is the surface friction matrix, 𝐺𝐺(𝑞𝑞) is the gravitational vector, 𝜏𝜏𝑑𝑑 denotes the bounded

unknown disturbances including the unstructured un-modeled dynamics, and 𝜆𝜆 (the Lagrange

multiplier) represents the vector of the constraint forces.

Characteristics of the dynamic model of the UGV mobile robot:

When modeling a UGV the following three properties must be satisfied:

47

Property 1: The inertia matrix is symmetric, 𝑀𝑀(𝑞𝑞) = 𝑀𝑀(𝑞𝑞)𝑇𝑇, positive definite.

Property 2: Inverse of the inertia matrix 𝑀𝑀(𝑞𝑞) exists and its positive definite.

Property 3: The matrix 𝑁𝑁(𝑞𝑞, �̇�𝑞) = �̇�𝑀(𝑞𝑞) − 2𝐶𝐶(𝑞𝑞, �̇�𝑞) is a skew matrix: That is

𝑁𝑁(𝑞𝑞, �̇�𝑞)𝑖𝑖𝑖𝑖𝑇𝑇 = −𝑁𝑁(𝑞𝑞, �̇�𝑞)𝑖𝑖𝑖𝑖.

The qualities of a skew symmetry matrix, 𝑁𝑁(𝑞𝑞, �̇�𝑞) = �̇�𝑀(𝑞𝑞) − 2𝐶𝐶(𝑞𝑞, �̇�𝑞) and the positive symmetry

matrix, 𝑀𝑀(𝑞𝑞) = 𝑀𝑀(𝑞𝑞)𝑇𝑇, will need to be verified to prove the controllability and asymptotic

stability of the nonholonomic mobile robot system.

To eliminate the nonholonomic constraint matrix 𝐴𝐴𝑇𝑇(𝑞𝑞)𝜆𝜆 and get a constraint free mathematical

model a null space state 𝑎𝑎(𝑎𝑎 −𝑚𝑚) matrix 𝑆𝑆(𝑞𝑞) which satisfy equation 5.6 was derived.

𝑆𝑆(𝑞𝑞)𝑇𝑇𝐴𝐴(𝑞𝑞)𝑇𝑇 = 0 (5.7)

From Eqns. (5.4) and (5.7), it is easy to obtain the velocity vector, �̇�𝑞, of the system:

�̇�𝑞 = 𝑆𝑆(𝑞𝑞)𝑣𝑣 (5.8)

Eqn.(5.8) can then be differentiated to find the system’s acceleration, �̈�𝑞:

�̈�𝑞 = �̇�𝑆(𝑞𝑞)𝑣𝑣 + 𝑆𝑆(𝑞𝑞)�̇�𝑣 (5.9)

Now by pre multiplying Eq.(5.6) by 𝑆𝑆(𝑞𝑞)𝑇𝑇 the following expression is obtained

𝑆𝑆(𝑞𝑞)𝑇𝑇 (𝑀𝑀(𝑞𝑞)�̈�𝑞 + 𝐶𝐶(𝑞𝑞, �̇�𝑞)�̇�𝑞 + 𝐹𝐹(�̇�𝑞) + 𝐺𝐺(𝑞𝑞) + 𝜏𝜏𝑑𝑑) = 𝑆𝑆(𝑞𝑞)𝑇𝑇𝐸𝐸(𝑞𝑞)𝜏𝜏 (5.10)

Now substituting Eqns. (5.8) and (5.9) into Eqn. (5.10) Eqn. (5.11) is obtained:

𝑆𝑆(𝑞𝑞)𝑇𝑇 (𝑀𝑀(𝑞𝑞)[�̇�𝑆(𝑞𝑞)𝑣𝑣 + 𝑆𝑆(𝑞𝑞)�̇�𝑣] + 𝐶𝐶(𝑞𝑞, �̇�𝑞)𝑆𝑆(𝑞𝑞)𝑣𝑣 + 𝐹𝐹(�̇�𝑞) + 𝐺𝐺(𝑞𝑞) + 𝜏𝜏𝑑𝑑) =

𝑆𝑆(𝑞𝑞)𝑇𝑇𝐸𝐸(𝑞𝑞)𝜏𝜏 (5.11)

By using suitable definitions typically used in robotics Eqn. (5.11) can be rewritten as follows:

𝑀𝑀�(𝑞𝑞)�̇�𝑣 + 𝐶𝐶̅(𝑞𝑞, �̇�𝑞)𝑣𝑣 + �̅�𝐺(𝑞𝑞) + 𝜏𝜏�̅�𝑑 = 𝐸𝐸�(𝑞𝑞)𝜏𝜏 (5.12)

where:

48

𝑀𝑀�(𝑞𝑞) = 𝑆𝑆𝑇𝑇(𝑞𝑞)𝑀𝑀(𝑞𝑞)𝑆𝑆(𝑞𝑞) (5.13)

𝐶𝐶̅(𝑞𝑞, �̇�𝑞) = 𝑆𝑆𝑇𝑇(𝑞𝑞)𝑀𝑀(𝑞𝑞)�̇�𝑆(𝑞𝑞)+𝑆𝑆𝑇𝑇(𝑞𝑞)𝐶𝐶(𝑞𝑞, �̇�𝑞)𝑆𝑆(𝑞𝑞) (5.14)

�̅�𝐺(𝑞𝑞) = 𝑆𝑆𝑇𝑇(𝑞𝑞)𝐺𝐺(𝑞𝑞) (5.15)

𝜏𝜏�̅�𝑑 = 𝑆𝑆𝑇𝑇(𝑞𝑞)𝜏𝜏𝑑𝑑 (5.16)

and

𝐸𝐸�(𝑞𝑞) = 𝑆𝑆𝑇𝑇(𝑞𝑞)𝐵𝐵(𝑞𝑞) (5.17)

The reduced model in Eqn. (5.12) defines the complete dynamic model of the nonholonomic

system in an easy control compact form. The model includes all constraints which have been

typically being neglected when controlling MMs.

5.3 Dynamic model of the nonholonomic UGV

The coordinates of the position of the center of gravity (COG) of the UGV, {B} Fig. (4.2), are

defined as:

𝑥𝑥𝑏𝑏 = 𝑥𝑥0 + 𝑙𝑙𝑏𝑏𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 (5.18)

𝑦𝑦𝑏𝑏 = 𝑦𝑦0 + 𝑙𝑙𝑏𝑏𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 (5.19)

The corresponding velocity of the vehicle’s COG to be used to estimate the corresponding

kinetic energy Equation is thus defined as:

�̇�𝑥𝑏𝑏 = �̇�𝑥0 − 𝑙𝑙𝑏𝑏�̇�𝑞𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 (5.20)

�̇�𝑦𝑏𝑏 = �̇�𝑦0 + 𝑙𝑙𝑏𝑏�̇�𝑞𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 (5.21)

From Eqn. (5.3) the kinetic energy of the nonholonomic UGV is:

𝑇𝑇 =12𝑚𝑚𝑏𝑏𝑣𝑣𝑏𝑏2 +

12𝐼𝐼𝑏𝑏�̇�𝑞2 (5.22)

where

49

𝑣𝑣𝑏𝑏2 = �̇�𝑥02 + �̇�𝑦02 − 2𝑥𝑥0̇𝑙𝑙𝑏𝑏�̇�𝑞𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 + 2𝑦𝑦0̇𝑙𝑙𝑏𝑏�̇�𝑞𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 + 𝑙𝑙𝑏𝑏2�̇�𝑞2

(5.23)

�̇�𝑞2 =(�̇�𝑞𝑟𝑟 − �̇�𝑞𝑙𝑙)2𝑟𝑟2

4𝑏𝑏2 (5.24)

resulting in the final kinetic energy computed using Eqn. (5.25).

𝑇𝑇 =12𝑚𝑚𝑏𝑏��̇�𝑥02 + �̇�𝑦02 − 2𝑥𝑥0̇𝑙𝑙𝑏𝑏�̇�𝑞𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 + 2𝑦𝑦0̇𝑙𝑙𝑏𝑏�̇�𝑞𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 + 𝑙𝑙𝑏𝑏

2�̇�𝑞2�

+12𝐼𝐼𝑏𝑏 �

(�̇�𝑞𝑟𝑟 − �̇�𝑞𝑙𝑙)2𝑟𝑟2

4𝑏𝑏2�

(5.25)

The step by step methodology used to obtain the dynamic equations using the above generalized

coordinates and Lagrangian is presented next:

𝜕𝜕𝜕𝜕𝜕𝜕�̇�𝑥0

= 𝑚𝑚𝑏𝑏�̇�𝑥0 − 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏�̇�𝑞𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 (5.26)

𝜕𝜕𝜕𝜕𝜕𝜕�̇�𝑦0

= 𝑚𝑚𝑏𝑏�̇�𝑦0 + 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏�̇�𝑞𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 (5.27)

𝜕𝜕𝜕𝜕𝜕𝜕�̇�𝑞

= 2𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏2�̇�𝑞 − 𝑚𝑚𝑏𝑏�̇�𝑥0𝑙𝑙𝑏𝑏𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 + 𝑚𝑚𝑏𝑏�̇�𝑦0𝑙𝑙𝑏𝑏𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 + 𝐼𝐼𝑏𝑏�̇�𝑞 (5.28)

The time derivatives of the above three equations result in:

𝑎𝑎𝑎𝑎𝑡𝑡�𝜕𝜕𝜕𝜕𝜕𝜕�̇�𝑥𝑐𝑐

� = 𝑚𝑚𝑏𝑏�̈�𝑥0 − 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞�̈�𝑞 − 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞�̇�𝑞2 (5.29)

𝑎𝑎𝑎𝑎𝑡𝑡�𝜕𝜕𝜕𝜕𝜕𝜕�̇�𝑦𝑐𝑐

� = 𝑚𝑚𝑏𝑏�̈�𝑦0 − 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏�̈�𝑞𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 − 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞�̇�𝑞2 (5.30)

50

𝑎𝑎𝑎𝑎𝑡𝑡�𝜕𝜕𝜕𝜕𝜕𝜕�̇�𝜃� = 𝐼𝐼𝑏𝑏�̈�𝑞 + 2𝑚𝑚𝑏𝑏𝐼𝐼𝑏𝑏�̈�𝑞2�̈�𝑞 + 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏�̈�𝑥𝑐𝑐𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 + 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏�̇�𝑥0�̇�𝑞𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 − 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏�̈�𝑦0𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞

+ 𝑚𝑚𝑏𝑏�̇�𝑦0𝑙𝑙𝑏𝑏�̇�𝑞𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞

(5.31)

The rest of the derivatives to complete the Lagrange equations are given as:

𝜕𝜕𝜕𝜕𝜕𝜕𝑥𝑥0

=𝜕𝜕𝜕𝜕𝜕𝜕𝑦𝑦0

= 0 (5.32)

𝜕𝜕𝜕𝜕𝜕𝜕𝑞𝑞

= 𝑚𝑚𝑏𝑏�̇�𝑥0𝑙𝑙𝑏𝑏�̇�𝑞𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 + 𝑚𝑚𝑏𝑏�̇�𝑦0𝑙𝑙𝑏𝑏�̇�𝑞𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 (5.33)

𝑎𝑎𝑎𝑎𝑡𝑡�𝜕𝜕𝜕𝜕𝜕𝜕�̇�𝑞� −

𝜕𝜕𝜕𝜕𝜕𝜕𝑞𝑞

= 𝐼𝐼𝑏𝑏 + 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏2�̈�𝑞 − 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏�̈�𝑥0𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 + 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏�̈�𝑦0𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 (5.34)

Substituting the mass, centrifugal, and Coriolis matrix in Eqn. (5.6) results in:

�𝑚𝑚𝑏𝑏 0 −𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞0 𝑚𝑚𝑏𝑏 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞

−𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 𝐼𝐼𝑏𝑏 + 𝑚𝑚𝑙𝑙𝑏𝑏2� �̈�𝑞 + �

−𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏�̇�𝑞2𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞−𝑚𝑚𝑏𝑏𝑙𝑙𝑏𝑏�̇�𝑞2𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞

0�

=1𝑟𝑟�𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞2𝑏𝑏 −2𝑏𝑏

� �𝜏𝜏𝑟𝑟𝜏𝜏𝑙𝑙 � − 𝐴𝐴𝑇𝑇(𝑞𝑞)𝜆𝜆

(5.35)

The Lagrange multiplier in Eqn. (5.35) can be eliminated by multiplying both sides of the

equation by 𝑆𝑆𝑇𝑇(𝑞𝑞) where 𝑆𝑆(𝑞𝑞)𝑇𝑇𝐴𝐴(𝑞𝑞)𝑇𝑇 = 0. This leads to the reduced dynamic model in Eqn.

(5.12).

The state space reduced model can be found by solving for �̇�𝒒 and �̇�𝒗 so the nonholonomic

constrained dynamic control equation of the UGV is given as:

51

�̇�𝑞 = 𝑆𝑆(𝑞𝑞)𝑣𝑣

�̇�𝑣 = 𝑀𝑀�(𝑞𝑞)−1[−𝐶𝐶̅(𝑞𝑞, �̇�𝑞)𝑣𝑣 − �̅�𝐺(𝑞𝑞) − 𝜏𝜏�̅�𝑑 + 𝐸𝐸�(𝑞𝑞)𝜏𝜏].

(5.36)

Having obtained the dynamic models presented above we can then define the dynamic model of

the MM as presented in the following section.

5.4 Dynamic modeling of the mobile manipulator (mobile platform + robot arm)

Assuming an evenly distributed mass on the MM the coordinates of the body’s center of mass of

the MM then the components of the MM’s position in terms of its frame of reference are:

𝑥𝑥𝑏𝑏 = 𝑥𝑥0 + 𝑙𝑙𝑏𝑏 cos 𝑞𝑞

𝑦𝑦𝑏𝑏 = 𝑦𝑦0 + 𝑙𝑙𝑏𝑏 sin 𝑞𝑞

𝑥𝑥𝑏𝑏 = 𝑥𝑥1

𝑦𝑦𝑏𝑏 = 𝑦𝑦1

(5.37)

where 𝑙𝑙𝑏𝑏 is the distance between the base of the arm and the center of gravity of the UGV.

𝑥𝑥2 = 𝑥𝑥0 + 𝑙𝑙𝑏𝑏 cos 𝑞𝑞 + 𝑟𝑟2 cos 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1)

𝑦𝑦2 = 𝑦𝑦0 + 𝑙𝑙𝑏𝑏 sin 𝑞𝑞 + 𝑟𝑟2 cos 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1)

𝑝𝑝2 = 𝑎𝑎1 + 𝑟𝑟2 sin 𝑞𝑞2

(5.38)

𝑥𝑥3 = 𝑥𝑥0 + 𝑙𝑙𝑏𝑏 cos 𝑞𝑞 + 𝑎𝑎2 cos𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) + 𝑟𝑟3 cos(𝑞𝑞2 + 𝑞𝑞3) cos(𝑞𝑞 + 𝑞𝑞1)

𝑦𝑦3 = 𝑦𝑦0 + 𝑙𝑙𝑏𝑏 sin 𝑞𝑞 + 𝑎𝑎2 cos 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) + 𝑟𝑟3 cos(𝑞𝑞2 + 𝑞𝑞3) sin(𝑞𝑞 + 𝑞𝑞1)

𝑝𝑝3 = 𝑎𝑎1 + 𝑎𝑎2 sin 𝑞𝑞2 + 𝑟𝑟3 sin(𝑞𝑞2 + 𝑞𝑞3)

(5.39)

52

The components of the MM’s velocity in terms of its frame of reference are:

�̇�𝑥𝑏𝑏 = �̇�𝑥 − 𝑙𝑙𝑏𝑏�̇�𝑞 sin 𝑞𝑞

�̇�𝑦𝑏𝑏 = �̇�𝑦 + 𝑙𝑙𝑏𝑏�̇�𝑞 cos𝑞𝑞 (5.40)

�̇�𝑥2 = �̇�𝑥 − 𝑙𝑙𝑏𝑏�̇�𝑞 sin 𝑞𝑞 − 𝑟𝑟2 sin 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) �̇�𝑞2 − 𝑟𝑟2 cos 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) (�̇�𝑞 + �̇�𝑞1)

�̇�𝑦2 = �̇�𝑦 + 𝑙𝑙𝑏𝑏�̇�𝑞 cos 𝑞𝑞 + 𝑟𝑟2 cos 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) (�̇�𝑞 + �̇�𝑞1) − 𝑟𝑟2 sin 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) �̇�𝑞2

�̇�𝑝2 = 𝑟𝑟2 cos𝑞𝑞2 �̇�𝑞2

(5.41)

�̇�𝑥3 = �̇�𝑥 − 𝑙𝑙𝑏𝑏�̇�𝑞 sin 𝑞𝑞 − 𝑎𝑎2 sin 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) �̇�𝑞2

− 𝑎𝑎2 cos 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) (�̇�𝑞 + �̇�𝑞1)

− 𝑟𝑟3 cos(𝑞𝑞2 + 𝑞𝑞3) sin(𝑞𝑞 + 𝑞𝑞1) (�̇�𝑞 + �̇�𝑞1)

− 𝑟𝑟3 cos(𝑞𝑞 + 𝑞𝑞1) sin(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞2 + �̇�𝑞3)

�̇�𝑦3 = �̇�𝑦 + 𝑙𝑙𝑏𝑏�̇�𝑞 cos 𝑞𝑞 + 𝑎𝑎2 cos 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) (�̇�𝑞 + �̇�𝑞1)

− 𝑎𝑎2 sin 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) �̇�𝑞2

+ 𝑟𝑟3 cos(𝑞𝑞 + 𝑞𝑞1) cos(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞 + �̇�𝑞1)

− 𝑟𝑟3 sin(𝑞𝑞 + 𝑞𝑞1) sin(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞2 + �̇�𝑞3)

�̇�𝑝3 = 𝑎𝑎2 cos 𝑞𝑞2 �̇�𝑞2 + 𝑟𝑟3 cos(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞2 + �̇�𝑞3)

(5.42)

With Eqns. (5.41 and 5.42) the kinetic energy of the MM can be computed. These equations are

to be used for combining the dynamics of the MMs into its model.

The kinetic energy is computed as:

53

𝑇𝑇 = 𝑇𝑇𝑚𝑚 + 𝑇𝑇1 + 𝑇𝑇2 + 𝑇𝑇3

=12𝑚𝑚𝑏𝑏(�̇�𝑥2 + �̇�𝑦2) +

12𝐼𝐼𝑏𝑏�̇�𝑞2 +

12𝑚𝑚1(�̇�𝑥12 + �̇�𝑦12) +

12𝐼𝐼1(�̇�𝑞 + �̇�𝑞1)2

+12𝑚𝑚2(�̇�𝑥22 + �̇�𝑦22 + �̇�𝑝22) +

12𝐼𝐼2 �(�̇�𝑞 + �̇�𝑞1)2 +

12𝐼𝐼2�̇�𝑞22�

+12𝑚𝑚3(�̇�𝑥32 + �̇�𝑦32 + �̇�𝑝32) +

12𝐼𝐼3(�̇�𝑞 + �̇�𝑞1)2 +

12𝐼𝐼3(�̇�𝑞12 + �̇�𝑞22)

(5.43)

The potential energy equation of the MMs is defined as:

𝑃𝑃 = 𝑚𝑚1𝑔𝑔𝑟𝑟1 + 𝑚𝑚2 𝑔𝑔( 𝑎𝑎1 + 𝑟𝑟2 sin(𝑞𝑞2))

+ 𝑔𝑔𝑚𝑚3� 𝑎𝑎1 + 𝑎𝑎2 sin(𝑞𝑞2) + 𝑟𝑟3 sin�(𝑞𝑞2) + (𝑞𝑞3) �� (5.44)

which leads to the following form which is a regular compact form used in traditional robotic

manipulator arm control:

𝑀𝑀(𝑞𝑞)�̈�𝑞 + 𝐶𝐶(𝑞𝑞, �̇�𝑞)�̇�𝑞 + 𝐹𝐹(�̇�𝑞) + 𝐺𝐺(𝑞𝑞) + 𝜏𝜏𝑑𝑑 = 𝐸𝐸(𝑞𝑞)𝜏𝜏 + 𝐴𝐴𝑇𝑇(𝑞𝑞)𝜆𝜆 (5.45)

where 𝑚𝑚𝑏𝑏 , and 𝐼𝐼𝑏𝑏 , are mass and moment of inertia of the platform 𝑚𝑚1,𝑚𝑚2,𝑚𝑚3and 𝐼𝐼1, 𝐼𝐼2, 𝐼𝐼3are

masses and moments of inertia of links 1,2 and 3, respectively, 𝐴𝐴(𝑞𝑞) is the nonholonomic

constraint, and 𝑆𝑆(𝑞𝑞) is the null state space matrix (Eqn. (5.5) with:

𝐴𝐴(𝑞𝑞) = [− sin 𝑞𝑞 cos𝑞𝑞 − 1 0 0 0]𝑇𝑇 (5.46)

and

54

𝑆𝑆(𝑞𝑞) =

⎣⎢⎢⎢⎢⎢⎢⎢⎡𝑟𝑟2

cos 𝑞𝑞 +𝑟𝑟

2𝑏𝑏 sin 𝑞𝑞

𝑟𝑟2

cos 𝑞𝑞 −𝑟𝑟

2𝑏𝑏 sin 𝑞𝑞 0 0 0

𝑟𝑟2

sin 𝑞𝑞 −𝑟𝑟

2𝑏𝑏 cos 𝑞𝑞

𝑟𝑟2

sin 𝑞𝑞 +𝑟𝑟

2𝑏𝑏 cos 𝑞𝑞 0 0 0

−𝑟𝑟

2𝑏𝑏000

𝑟𝑟2𝑏𝑏000

0100

0010

0001⎦⎥⎥⎥⎥⎥⎥⎥⎤

(5.47)

The inertia matrix 𝑀𝑀(𝑞𝑞) of the dynamics of the MMs is defined as:

𝑀𝑀(𝑞𝑞) =

⎣⎢⎢⎢⎢⎡𝑀𝑀11 𝑀𝑀12 𝑀𝑀13 𝑀𝑀14 𝑀𝑀15 𝑀𝑀16𝑀𝑀21 𝑀𝑀22 𝑀𝑀23 𝑀𝑀24 𝑀𝑀25 𝑀𝑀26𝑀𝑀31𝑀𝑀41𝑀𝑀51𝑀𝑀61

𝑀𝑀32𝑀𝑀42𝑀𝑀52𝑀𝑀62



𝑀𝑀35 𝑀𝑀36𝑀𝑀45 𝑀𝑀46𝑀𝑀55 𝑀𝑀56𝑀𝑀65 𝑀𝑀66⎦

⎥⎥⎥⎥⎤

(5.48)

where:

𝑀𝑀11 = 𝑀𝑀22 = 𝑚𝑚𝑏𝑏 + 𝑚𝑚1 + 𝑚𝑚2 + 𝑚𝑚3,

𝑀𝑀12 = 𝑀𝑀21 = 0,

𝑀𝑀13 = 𝑀𝑀31 = −𝑎𝑎 sin 𝑞𝑞 (𝑚𝑚𝑏𝑏 + 𝑚𝑚1 + 𝑚𝑚2 + 𝑚𝑚3) − sin(𝑞𝑞 + 𝑞𝑞1) cos 𝑞𝑞2 (𝑚𝑚3𝑎𝑎2 + 𝑚𝑚2𝑟𝑟2) −

𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2 + 𝑞𝑞3) sin(𝑞𝑞 + 𝑞𝑞1),

𝑀𝑀14 = 𝑀𝑀41 = − sin(𝑞𝑞 + 𝑞𝑞1) cos 𝑞𝑞2 (𝑚𝑚3𝑎𝑎2 + 𝑚𝑚2𝑟𝑟2) + 𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2 + 𝑞𝑞3) sin(𝑞𝑞 + 𝑞𝑞1),

𝑀𝑀15 = 𝑀𝑀51 = − cos(𝑞𝑞 + 𝑞𝑞1) sin 𝑞𝑞2 (𝑚𝑚3𝑎𝑎2 + 𝑚𝑚2𝑟𝑟2) −𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞2 + 𝑞𝑞3) cos(𝑞𝑞 + 𝑞𝑞1),

𝑀𝑀16 = 𝑀𝑀61 = −𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞2 + 𝑞𝑞3) cos(𝑞𝑞 + 𝑞𝑞1),

𝑀𝑀23 = 𝑀𝑀32 = 𝑎𝑎 cos𝑞𝑞 (𝑚𝑚𝑏𝑏 + 𝑚𝑚1 + 𝑚𝑚2 + 𝑚𝑚3) + cos(𝑞𝑞 + 𝑞𝑞1) cos 𝑞𝑞2 (𝑚𝑚3𝑎𝑎2 + 𝑚𝑚2𝑟𝑟2)

+ 𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2 + 𝑞𝑞3) cos(𝑞𝑞 + 𝑞𝑞1)

𝑀𝑀24 = 𝑀𝑀42 = cos(𝑞𝑞 + 𝑞𝑞1) cos 𝑞𝑞2 (𝑚𝑚3𝑎𝑎2 + 𝑚𝑚2𝑟𝑟2) + 𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2 + 𝑞𝑞3) cos(𝑞𝑞 + 𝑞𝑞1),

𝑀𝑀25 = 𝑀𝑀52 = − sin(𝑞𝑞 + 𝑞𝑞1) sin 𝑞𝑞2 (𝑚𝑚3𝑎𝑎2 + 𝑚𝑚2𝑟𝑟2) −𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞2 + 𝑞𝑞3) sin(𝑞𝑞 + 𝑞𝑞1),

𝑀𝑀26 = 𝑀𝑀62 = −𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞2 + 𝑞𝑞3) sin(𝑞𝑞 + 𝑞𝑞1),

55

𝑀𝑀33 = 𝐼𝐼𝑏𝑏 + 𝐼𝐼1 + 𝐼𝐼2 + 𝐼𝐼3 + 𝑎𝑎2(𝑚𝑚𝑏𝑏 + 𝑚𝑚1 + 𝑚𝑚2 + 𝑚𝑚3) + 𝑚𝑚3𝑟𝑟32 + 𝑎𝑎22𝑚𝑚3 cos𝑞𝑞22 +

𝑚𝑚2𝑟𝑟22 cos 𝑞𝑞22 − 𝑚𝑚3𝑟𝑟32 cos 𝑞𝑞22 − 𝑚𝑚3𝑟𝑟32 cos𝑞𝑞32 + 2𝑚𝑚3𝑟𝑟32 cos 𝑞𝑞22 cos 𝑞𝑞32 +

2 𝑎𝑎𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞1) cos(𝑞𝑞2) + 2𝑎𝑎𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞1) cos(𝑞𝑞2) + 2𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos 𝑞𝑞22 cos(𝑞𝑞3) +

2 𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) cos(𝑞𝑞2) cos(𝑞𝑞3) − 2 𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) sin(𝑞𝑞2) sin(𝑞𝑞3) −

− 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2) sin(𝑞𝑞2) sin(𝑞𝑞3) − 2𝑚𝑚3𝑟𝑟32 cos(𝑞𝑞2) cos(𝑞𝑞3) sin(𝑞𝑞2) sin(𝑞𝑞3),

𝑀𝑀34 = 𝑀𝑀43 = 𝐼𝐼1 + 𝐼𝐼2 + 𝐼𝐼3 + 𝑚𝑚3𝑟𝑟32 + 𝑎𝑎22𝑚𝑚3 cos𝑞𝑞22 + 𝑚𝑚2𝑟𝑟22 cos 𝑞𝑞22 − 𝑚𝑚3𝑟𝑟32 cos 𝑞𝑞22 −

𝑚𝑚3𝑟𝑟32 cos 𝑞𝑞32 + 2𝑚𝑚3𝑟𝑟32 cos 𝑞𝑞22 cos 𝑞𝑞32 + 𝑎𝑎𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞1) cos(𝑞𝑞2) + 𝑎𝑎𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞1) cos(𝑞𝑞2) +

2𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos𝑞𝑞22 cos(𝑞𝑞3) + 𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) cos(𝑞𝑞2) cos(𝑞𝑞3) −

𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) sin(𝑞𝑞2) sin(𝑞𝑞3) − 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2) sin(𝑞𝑞2) sin(𝑞𝑞3) −

2𝑚𝑚3𝑟𝑟32 cos(𝑞𝑞2) cos(𝑞𝑞3) sin(𝑞𝑞2) sin(𝑞𝑞3),

𝑀𝑀35 = 𝑀𝑀53 = − sin(𝑞𝑞1) sin 𝑞𝑞2 (𝑚𝑚3𝑎𝑎2 + 𝑚𝑚2𝑟𝑟2) −𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞2 + 𝑞𝑞3) sin (𝑞𝑞1),

𝑀𝑀36 = 𝑀𝑀63 = −𝑎𝑎𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞2 + 𝑞𝑞3) sin(𝑞𝑞1),

𝑀𝑀44 = 𝐼𝐼1 + 𝐼𝐼2 + 𝐼𝐼3 + 1/2(𝑎𝑎22𝑚𝑚3 + 𝑚𝑚2𝑟𝑟22 + 𝑚𝑚3𝑟𝑟32 + 𝑎𝑎22𝑚𝑚3 cos(2𝑞𝑞2) + 𝑚𝑚2𝑟𝑟22 cos(2𝑞𝑞2)

+ 𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞2 + 2𝑞𝑞3)) + 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(2𝑞𝑞2 + 𝑞𝑞3) + 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞3)

𝑀𝑀45 = 𝑀𝑀54 = 𝑀𝑀46 = 𝑀𝑀64 = 0

𝑀𝑀55 = 𝐼𝐼2 + 𝐼𝐼3 + 1/2(𝑎𝑎22𝑚𝑚3 + 𝑚𝑚2𝑟𝑟22 + 𝑚𝑚3𝑟𝑟32 + 𝑎𝑎22𝑚𝑚3 cos(2𝑞𝑞2) + 𝑚𝑚2𝑟𝑟22 cos(2𝑞𝑞2)

+ 𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞2 + 2𝑞𝑞3)) − 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(2𝑞𝑞2 + 𝑞𝑞3) + 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞3)

𝑀𝑀56 = 𝑀𝑀65 = 𝐼𝐼3 + 1/2( 𝑚𝑚3𝑟𝑟32 − 𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞2 + 2𝑞𝑞3) − 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(2𝑞𝑞2 + 𝑞𝑞3)

+ 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞3))

and

𝑀𝑀66 = 𝐼𝐼3 + 𝑚𝑚3𝑟𝑟32𝑠𝑠𝑖𝑖𝑎𝑎2(𝑞𝑞2 + 𝑞𝑞3)

56

To complete the terms needed in Equation (5.46) the Coriolis and Centrifugal matrix, (𝑞𝑞, �̇�𝑞), is

derived as shown in Equation (5.49).

𝐶𝐶(𝑞𝑞, �̇�𝑞) =

⎣⎢⎢⎢⎢⎡𝐶𝐶11 𝐶𝐶12 𝐶𝐶13 𝐶𝐶14 𝐶𝐶15 𝐶𝐶16𝐶𝐶21 𝐶𝐶22 𝐶𝐶23 𝐶𝐶24 𝐶𝐶25 𝐶𝐶26𝐶𝐶31𝐶𝐶41𝐶𝐶51𝐶𝐶61

𝐶𝐶32𝐶𝐶42𝐶𝐶52𝐶𝐶62



𝐶𝐶35 𝐶𝐶36𝐶𝐶45 𝐶𝐶46𝐶𝐶55 𝐶𝐶56𝐶𝐶65 𝐶𝐶66⎦

⎥⎥⎥⎥⎤

(5.49)

where:

𝐶𝐶11 = 𝐶𝐶12 = 𝐶𝐶21 = 𝐶𝐶22 = 𝐶𝐶31 = 𝐶𝐶32 = 0,

𝐶𝐶13 = −1/2𝑚𝑚2(4𝑎𝑎 cos 𝑞𝑞 �̇�𝑞 + 2𝑟𝑟2 cos 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) (2�̇�𝑞 + 2𝑞𝑞1̇) − 4𝑟𝑟2 sin 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) 𝑞𝑞2̇ −

1/2𝑚𝑚3(4𝑎𝑎 cos 𝑞𝑞 �̇�𝑞 + 2𝑎𝑎2 cos 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) (2�̇�𝑞 + 2𝑞𝑞1̇) − 4𝑎𝑎2 sin 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1)𝑞𝑞2̇) +

2𝑟𝑟3 cos(𝑞𝑞 + 𝑞𝑞1) cos(𝑞𝑞2 + 𝑞𝑞3) (2�̇�𝑞 + 2𝑞𝑞1̇) − 4𝑟𝑟3 sin(𝑞𝑞 + 𝑞𝑞1) sin(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞2 + �̇�𝑞3)) −

2𝑎𝑎𝑚𝑚𝑏𝑏 cos 𝑞𝑞 �̇�𝑞 − 2𝑎𝑎𝑚𝑚1 cos 𝑞𝑞 �̇�𝑞,

𝐶𝐶14 = −1/2𝑚𝑚3(2𝑎𝑎2 cos(𝑞𝑞 + 𝑞𝑞1) (2�̇�𝑞 + 2𝑞𝑞1̇) − 4𝑎𝑎2 sin 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) 𝑞𝑞2̇

+ 2𝑟𝑟3 cos(𝑞𝑞 + 𝑞𝑞1) cos(𝑞𝑞2 + 𝑞𝑞3) (2�̇�𝑞 + 2𝑞𝑞1̇) − 4𝑟𝑟3 sin(𝑞𝑞 + 𝑞𝑞1) sin(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞2

+ �̇�𝑞3)) − 1/2𝑚𝑚2(2𝑟𝑟2 cos 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) (2�̇�𝑞 + 2𝑞𝑞1̇) − 4𝑟𝑟2 sin 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) 𝑞𝑞2̇,

𝐶𝐶15 = −1/2𝑚𝑚2(4𝑟𝑟2 cos𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) 𝑞𝑞2̇ − 4𝑟𝑟2 sin 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) (�̇�𝑞 + 𝑞𝑞1̇) −

1/2𝑚𝑚3(2𝑎𝑎2 cos 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) 𝑞𝑞2̇ + 2𝑟𝑟3 cos(𝑞𝑞 + 𝑞𝑞1) cos(𝑞𝑞2 + 𝑞𝑞3) (2�̇�𝑞2 + 2�̇�𝑞3) −

−4𝑎𝑎2 sin 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) (�̇�𝑞 + 𝑞𝑞1̇) − 4𝑟𝑟3 sin(𝑞𝑞 + 𝑞𝑞1) sin(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇)),

𝐶𝐶16 = 1/2𝑚𝑚3(2𝑟𝑟3 cos(𝑞𝑞 + 𝑞𝑞1) cos(𝑞𝑞2 + 𝑞𝑞3) (2�̇�𝑞2 + 2�̇�𝑞3) − 4𝑟𝑟3 sin(𝑞𝑞 + 𝑞𝑞1) sin(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞 +

𝑞𝑞1̇)),

57

𝐶𝐶23 = −1/2𝑚𝑚2(4𝑎𝑎 sin 𝑞𝑞 �̇�𝑞 + 2𝑟𝑟2 cos 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) (2�̇�𝑞 + 2𝑞𝑞1̇) + 4𝑟𝑟2 sin 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) 𝑞𝑞2̇ −

1/2𝑚𝑚3(4𝑎𝑎 sin 𝑞𝑞 �̇�𝑞 + 2𝑎𝑎2 cos 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) (2�̇�𝑞 + 2𝑞𝑞1̇) + 4𝑎𝑎2 sin 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) 𝑞𝑞2̇) +

2𝑟𝑟3 sin(𝑞𝑞 + 𝑞𝑞1) cos(𝑞𝑞2 + 𝑞𝑞3) (2�̇�𝑞 + 2𝑞𝑞1̇) + 4𝑟𝑟3 cos(𝑞𝑞 + 𝑞𝑞1) sin(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞2 + �̇�𝑞3)) −

2𝑎𝑎𝑚𝑚𝑏𝑏 sin 𝑞𝑞 �̇�𝑞 − 2𝑎𝑎𝑚𝑚1 sin 𝑞𝑞 �̇�𝑞,

𝐶𝐶24 = −1/2𝑚𝑚3(2𝑎𝑎2 𝑐𝑐𝑐𝑐𝑠𝑠 𝑞𝑞2 𝑠𝑠𝑖𝑖𝑎𝑎(𝑞𝑞 + 𝑞𝑞1) (2�̇�𝑞 + 2𝑞𝑞1̇) + 4𝑎𝑎2 𝑠𝑠𝑖𝑖𝑎𝑎 𝑞𝑞2 𝑐𝑐𝑐𝑐𝑠𝑠(𝑞𝑞 + 𝑞𝑞1) 𝑞𝑞2̇

+ 2𝑟𝑟3 𝑠𝑠𝑖𝑖𝑎𝑎(𝑞𝑞 + 𝑞𝑞1) 𝑐𝑐𝑐𝑐𝑠𝑠(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇) + 4𝑟𝑟3 𝑐𝑐𝑐𝑐𝑠𝑠(𝑞𝑞 + 𝑞𝑞1) 𝑠𝑠𝑖𝑖𝑎𝑎(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞2

+ �̇�𝑞3)) − 1/2𝑚𝑚2(2𝑟𝑟2 𝑐𝑐𝑐𝑐𝑠𝑠 𝑞𝑞2 𝑠𝑠𝑖𝑖𝑎𝑎(𝑞𝑞 + 𝑞𝑞1) (2�̇�𝑞 + 2𝑞𝑞1̇) + 4𝑟𝑟2 𝑠𝑠𝑖𝑖𝑎𝑎 𝑞𝑞2 𝑐𝑐𝑐𝑐𝑠𝑠(𝑞𝑞 + 𝑞𝑞1) 𝑞𝑞2̇

𝐶𝐶25 = −1/2𝑚𝑚2(4𝑟𝑟2 cos 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) 𝑞𝑞2̇ + 4𝑟𝑟2 sin 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) (�̇�𝑞 + 𝑞𝑞1̇) −

1/2𝑚𝑚3(4𝑎𝑎2 cos 𝑞𝑞2 sin(𝑞𝑞 + 𝑞𝑞1) 𝑞𝑞2̇ + 2𝑟𝑟3 cos(𝑞𝑞 + 𝑞𝑞1) cos(𝑞𝑞2 + 𝑞𝑞3) (2�̇�𝑞2 + 2�̇�𝑞3) ∓

4𝑎𝑎2 sin 𝑞𝑞2 cos(𝑞𝑞 + 𝑞𝑞1) (�̇�𝑞 + 𝑞𝑞1̇) + 4𝑟𝑟3 cos(𝑞𝑞 + 𝑞𝑞1) sin(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇)),

𝐶𝐶26 = 1/2𝑚𝑚3(2𝑟𝑟3 sin(𝑞𝑞 + 𝑞𝑞1) cos(𝑞𝑞2 + 𝑞𝑞3) (2�̇�𝑞2 + 2�̇�𝑞3) + 4𝑟𝑟3 cos(𝑞𝑞 + 𝑞𝑞1) sin(𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞 +

𝑞𝑞1̇)),

𝐶𝐶33 = 2𝑎𝑎𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞1) sin(𝑞𝑞2) sin(𝑞𝑞3)𝑞𝑞1̇ − 𝑚𝑚2𝑟𝑟22 sin(2𝑞𝑞2) 𝑞𝑞2̇ − 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3) 𝑞𝑞3̇

− 𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞2) sin(2𝑞𝑞3) (𝑞𝑞2̇ + 𝑞𝑞3̇) −𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞3) sin(2𝑞𝑞2) (𝑞𝑞2̇ + 𝑞𝑞3̇)

− 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞3) sin(2𝑞𝑞2) (𝑞𝑞2̇ + 𝑞𝑞3̇) − 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3) cos(2𝑞𝑞2) (𝑞𝑞2̇

+ 𝑞𝑞3̇) − 2𝑎𝑎 𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞2) sin(𝑞𝑞1)𝑞𝑞1̇ − 2𝑎𝑎 𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞1) sin(𝑞𝑞2) 𝑞𝑞2̇

− 2𝑎𝑎𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞2) sin(𝑞𝑞1)𝑞𝑞1̇ − 2𝑎𝑎𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞1) sin(𝑞𝑞2) 𝑞𝑞2̇

− 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2) cos(𝑞𝑞3) sin(𝑞𝑞1)𝑞𝑞1̇ − 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) cos(𝑞𝑞2) sin(𝑞𝑞3)(𝑞𝑞2̇

+ 𝑞𝑞3̇) − 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) cos(𝑞𝑞3) sin(𝑞𝑞2) (𝑞𝑞2̇ + 𝑞𝑞3̇) − 𝑎𝑎22𝑚𝑚3 sin(2𝑞𝑞2) 𝑞𝑞2̇

58

𝐶𝐶33 = 2𝑎𝑎𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞1) sin(𝑞𝑞2) sin(𝑞𝑞3)𝑞𝑞1̇ − 𝑚𝑚2𝑟𝑟22 sin(2𝑞𝑞2) 𝑞𝑞2̇ − 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3) 𝑞𝑞3̇

− 𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞2) sin(2𝑞𝑞3) (𝑞𝑞2̇ + 𝑞𝑞3̇) −𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞3) sin(2𝑞𝑞2) (𝑞𝑞2̇ + 𝑞𝑞3̇)

− 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞3) sin(2𝑞𝑞2) (𝑞𝑞2̇ + 𝑞𝑞3̇) − 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3) cos(2𝑞𝑞2) (𝑞𝑞2̇

+ 𝑞𝑞3̇) − 2𝑎𝑎 𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞2) sin(𝑞𝑞1)𝑞𝑞1̇ − 2𝑎𝑎 𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞1) sin(𝑞𝑞2) 𝑞𝑞2̇

− 2𝑎𝑎𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞2) sin(𝑞𝑞1)𝑞𝑞1̇ − 2𝑎𝑎𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞1) sin(𝑞𝑞2) 𝑞𝑞2̇

− 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2) cos(𝑞𝑞3) sin(𝑞𝑞1)𝑞𝑞1̇ − 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) cos(𝑞𝑞2) sin(𝑞𝑞3)(𝑞𝑞2̇

+ 𝑞𝑞3̇) − 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) cos(𝑞𝑞3) sin(𝑞𝑞2) (𝑞𝑞2̇ + 𝑞𝑞3̇) − 𝑎𝑎22𝑚𝑚3 sin(2𝑞𝑞2) 𝑞𝑞2̇

𝐶𝐶34 = 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3) 𝑞𝑞2̇ − 2𝑎𝑎22𝑚𝑚3 cos(2𝑞𝑞2) sin(2𝑞𝑞2) 𝑞𝑞2̇ − 2𝑚𝑚2𝑟𝑟22 cos(2𝑞𝑞2) sin(2𝑞𝑞2) 𝑞𝑞2̇

+ 2𝑚𝑚3𝑟𝑟32 cos(𝑞𝑞2) sin(𝑞𝑞2) (𝑞𝑞2̇ + 𝑞𝑞3̇) + 2𝑚𝑚3𝑟𝑟32 cos(𝑞𝑞3) sin(𝑞𝑞3) (𝑞𝑞2̇ + 𝑞𝑞3̇)

− 4 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos2 𝑞𝑞2 sin(𝑞𝑞3) (𝑞𝑞2̇ + 𝑞𝑞3̇)

− 4𝑚𝑚3𝑟𝑟32 cos(𝑞𝑞2) cos2 𝑞𝑞3 sin(𝑞𝑞2) (𝑞𝑞2̇ + 𝑞𝑞3̇)

− 4 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos2 𝑞𝑞2 cos(𝑞𝑞3) sin(𝑞𝑞3) 𝑞𝑞2̇ − 4𝑚𝑚3𝑟𝑟32 cos2 𝑞𝑞2 cos(𝑞𝑞3) sin(𝑞𝑞3)𝑞𝑞3̇

− 2𝑎𝑎 𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞2) sin(𝑞𝑞1) (�̇�𝑞 + 𝑞𝑞1̇ ) − 2𝑎𝑎 𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞1) sin(𝑞𝑞2) 𝑞𝑞2̇

− 2𝑎𝑎𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞2) sin(𝑞𝑞1) (�̇�𝑞 + 𝑞𝑞1̇ ) − 2𝑎𝑎𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞1) sin(𝑞𝑞2) 𝑞𝑞2̇

− 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2) cos(𝑞𝑞3) sin(𝑞𝑞1) (�̇�𝑞 + 𝑞𝑞1̇ )

− 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) cos(𝑞𝑞2) sin(𝑞𝑞3) (𝑞𝑞2̇ + 𝑞𝑞3̇)


− 4 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2) cos(𝑞𝑞3) sin(𝑞𝑞2) 𝑞𝑞2̇ − 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2) cos(𝑞𝑞3) sin(𝑞𝑞2) 𝑞𝑞3̇

+ 2𝑎𝑎𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞1) sin(𝑞𝑞2) sin(𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇ )

59

𝐶𝐶35 = 2𝑎𝑎𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞1) sin(𝑞𝑞2) sin(𝑞𝑞3) (𝑞𝑞2̇ + 𝑞𝑞3̇) − 𝑎𝑎22𝑚𝑚3 sin(2𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ )

− 𝑚𝑚2𝑟𝑟22 sin(2𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ ) −𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞2) sin(2𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇ )

−𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞3) sin(2𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ ) − 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞3) sin(2𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ )

− 2𝑎𝑎𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3) cos(2𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ ) − 2𝑎𝑎 𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞1) sin(𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ )

− 2𝑎𝑎 𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞2) sin(𝑞𝑞1)𝑞𝑞2̇ − 2𝑎𝑎𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞1) sin(𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ )

− 2𝑎𝑎𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞2) sin(𝑞𝑞1)𝑞𝑞2̇ − 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) cos(𝑞𝑞2) sin(𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇ )

− 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) cos(𝑞𝑞3) sin(𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ )


𝐶𝐶36 = 𝑎𝑎𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞1 − 𝑞𝑞2 − 𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇ ) − 𝑚𝑚3𝑟𝑟32 sin(2𝑞𝑞2 + 2𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇ )

− 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(2𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇ )

− 𝑎𝑎𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞1 − 𝑞𝑞2 − 𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇ )(𝑞𝑞2̇ + 𝑞𝑞3̇) − 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇ )

− 𝑎𝑎𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞1 + 𝑞𝑞2 + 𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇ ) − 𝑎𝑎𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞1 + 𝑞𝑞2 + 𝑞𝑞3) (𝑞𝑞2̇ + 𝑞𝑞3̇)

𝐶𝐶43 = 2𝑎𝑎 𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞2) sin(𝑞𝑞1) �̇�𝑞 −𝑚𝑚2𝑟𝑟22 sin(2𝑞𝑞2) 𝑞𝑞2̇ − 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3)𝑞𝑞3̇

−𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞2) sin(2𝑞𝑞3) (𝑞𝑞2̇ + 𝑞𝑞3̇) −𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞3) sin(2𝑞𝑞2) (𝑞𝑞2̇ + 𝑞𝑞3̇)

− 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞3) sin(2𝑞𝑞2) (𝑞𝑞2̇ + 𝑞𝑞3̇)

− 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3) cos(2𝑞𝑞2) (𝑞𝑞2̇ + 𝑞𝑞3̇) − 𝑎𝑎22𝑚𝑚3 sin(2𝑞𝑞2) 𝑞𝑞2̇

+ 2𝑎𝑎𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞2) sin(𝑞𝑞1) �̇�𝑞 + 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞2) cos(𝑞𝑞3) sin(𝑞𝑞1) �̇�𝑞

− 2𝑎𝑎𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞1) sin(𝑞𝑞2) sin(𝑞𝑞3) �̇�𝑞

𝐶𝐶44 = −𝑎𝑎22𝑚𝑚3 sin(2𝑞𝑞2) 𝑞𝑞2̇ −𝑚𝑚2𝑟𝑟22 sin(2𝑞𝑞2) 𝑞𝑞2̇ −𝑚𝑚3𝑟𝑟32 sin�2(𝑞𝑞2) + 2(𝑞𝑞3)� (𝑞𝑞2̇ + 𝑞𝑞3̇)

− 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin�2(𝑞𝑞2) + (𝑞𝑞3)� 𝑞𝑞2̇ − 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin�2(𝑞𝑞2) + (𝑞𝑞3)� 𝑞𝑞3̇

− 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3) 𝑞𝑞3̇

60

𝐶𝐶45 = −(�̇�𝑞 + 𝑞𝑞1̇ )�𝑚𝑚3 sin(2𝑞𝑞2)𝑎𝑎22 + 2𝑚𝑚3 sin�2(𝑞𝑞2) + (𝑞𝑞3)� 𝑎𝑎2𝑟𝑟3 + 𝑚𝑚2 sin(2𝑞𝑞2) 𝑟𝑟22

+ 𝑚𝑚3 sin�2(𝑞𝑞2) + 2(𝑞𝑞3)� 𝑟𝑟32�

𝐶𝐶46 = −𝑚𝑚3𝑟𝑟3(�̇�𝑞 + 𝑞𝑞1̇ )� 𝑎𝑎2 sin(𝑞𝑞3) + 𝑟𝑟3 sin�2(𝑞𝑞2) + 2(𝑞𝑞3)� + 𝑎𝑎2 sin�2(𝑞𝑞2) + (𝑞𝑞3)��

𝐶𝐶53 = 𝐶𝐶63 = 𝑎𝑎22𝑚𝑚3 sin(2𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ ) + 𝑚𝑚2𝑟𝑟22 sin(2𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ )

+ 𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞2) sin(2𝑞𝑞3) (�̇�𝑞 + 𝑞𝑞1̇ ) + 𝑚𝑚3𝑟𝑟32 cos(2𝑞𝑞3) sin(2𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ )

+ 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞3) sin(2𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ ) + 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3) cos(2𝑞𝑞2) (�̇�𝑞 + 𝑞𝑞1̇ )

+ 2𝑎𝑎 𝑎𝑎2𝑚𝑚3 cos(𝑞𝑞1) sin(𝑞𝑞2) �̇�𝑞 + 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) sin(𝑞𝑞2) �̇�𝑞

+ 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) cos(𝑞𝑞2) sin(𝑞𝑞3) �̇�𝑞 + 2𝑎𝑎𝑚𝑚3𝑟𝑟3 cos(𝑞𝑞1) cos(𝑞𝑞3) sin(𝑞𝑞2) �̇�𝑞

𝐶𝐶54 = 𝐶𝐶64 = (�̇�𝑞 + 𝑞𝑞1̇ )�𝑎𝑎22𝑚𝑚3 sin(2𝑞𝑞2) + 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin�2(𝑞𝑞2) + (𝑞𝑞3)� + 𝑚𝑚2𝑟𝑟22 sin(2𝑞𝑞2)

+ 𝑚𝑚3𝑟𝑟32 sin�2(𝑞𝑞2) + 2(𝑞𝑞3)��

𝐶𝐶55 = 𝐶𝐶65 = 𝑎𝑎22𝑚𝑚3 sin(2𝑞𝑞2) 𝑞𝑞2̇ + 𝑚𝑚2𝑟𝑟22 sin(2𝑞𝑞2) 𝑞𝑞2̇ + 𝑚𝑚3𝑟𝑟32 sin�2(𝑞𝑞2) + 2(𝑞𝑞3)� (𝑞𝑞2̇ + 𝑞𝑞3̇)

+ 2 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin�2(𝑞𝑞2) + (𝑞𝑞3)� (2𝑞𝑞2̇ + 𝑞𝑞3̇) − 𝑎𝑎2𝑚𝑚3𝑟𝑟3 sin(𝑞𝑞3) 𝑞𝑞3̇

and

𝐶𝐶56 = 𝐶𝐶66 = 𝑚𝑚3𝑟𝑟3(𝑞𝑞2̇ + 𝑞𝑞3̇)�𝑟𝑟3 sin�2(𝑞𝑞2) + 2(𝑞𝑞3)� − 𝑎𝑎2 sin(𝑞𝑞3) + 𝑎𝑎2 sin�2(𝑞𝑞2) + (𝑞𝑞3)��

Equation (5.45) also requires the non-singular actuator matrix, 𝐸𝐸(𝑞𝑞), which for the intended MM

is defined as follow using the nonholonomic constraints of the UGV:

61

𝐸𝐸(𝑞𝑞) =

⎣⎢⎢⎢⎢⎢⎢⎢⎡1𝑟𝑟

cos 𝑞𝑞1𝑟𝑟

cos 𝑞𝑞 0 0 01𝑟𝑟

sin 𝑞𝑞1𝑟𝑟

sin 𝑞𝑞 0 0 0

−𝑟𝑟

2𝑏𝑏000

𝑟𝑟2𝑏𝑏000

0100

0010

0001⎦⎥⎥⎥⎥⎥⎥⎥⎤

(5.50)

𝜏𝜏 = [𝜏𝜏𝑟𝑟 𝜏𝜏𝑙𝑙 𝜏𝜏1 𝜏𝜏2 𝜏𝜏3]𝑇𝑇 (5.51)

where 𝜏𝜏𝑟𝑟 and 𝜏𝜏𝑙𝑙 are the driving motor torques of the right and left wheel, and 𝜏𝜏1, 𝜏𝜏2 and 𝜏𝜏3 are

the three joint link manipulator torques.

The gravitational term of the MM including UGV and robot arm is given as:

𝐺𝐺(𝑞𝑞) = �𝜕𝜕𝑃𝑃𝜕𝜕𝑥𝑥

𝜕𝜕𝑃𝑃𝜕𝜕𝑦𝑦

𝜕𝜕𝑃𝑃𝜕𝜕𝑞𝑞1



�𝑇𝑇

(5.52)

where: 𝜕𝜕𝐺𝐺𝜕𝜕𝑥𝑥

=𝜕𝜕𝐺𝐺𝜕𝜕𝑦𝑦

=𝜕𝜕𝐺𝐺𝜕𝜕𝑞𝑞1

= 0

𝜕𝜕𝐺𝐺𝜕𝜕𝑞𝑞2

= 𝑔𝑔𝑚𝑚3� 𝑎𝑎2 cos(𝑞𝑞2) + 𝑟𝑟3 cos�(𝑞𝑞2) + (𝑞𝑞3) �� + 𝑔𝑔𝑚𝑚2𝑟𝑟2 cos(𝑞𝑞2)

and

𝜕𝜕𝐺𝐺𝜕𝜕𝑞𝑞3

= 𝑔𝑔𝑚𝑚3𝑟𝑟3 cos�(𝑞𝑞2) + (𝑞𝑞3) �

The Lagrange multiplier in Equation (5.45) can be eliminated by multiplying both sides of

Equation (5,45) by the null state matrix, 𝑆𝑆𝑇𝑇(𝑞𝑞), where 𝑆𝑆(𝑞𝑞)𝑇𝑇𝐴𝐴(𝑞𝑞)𝑇𝑇 = 0. This leads to the

reduced dynamic model in Equation (5.12)

62

As before the above equations have the three properties mentioned in Section 5.2 (e.g., Property

1, Property 2 and Property 3).

The properties of the used skew symmetry matrix if 𝑁𝑁(𝑞𝑞, �̇�𝑞) = �̇�𝑀(𝑞𝑞) − 2𝐶𝐶(𝑞𝑞, �̇�𝑞) and positive

symmetry matrix 𝑀𝑀(𝑞𝑞) = 𝑀𝑀(𝑞𝑞)𝑇𝑇prove the controllability and asymptotic stability of the

nonholonomic MM.

As before, the state space reduced model found by solving for �̇�𝒒 and �̇�𝒗. The result is the

nonholonomic constrained dynamic control equation of the MM defined as:


�̇�𝑣 = 𝑀𝑀�(𝑞𝑞)−1[−𝐶𝐶̅(𝑞𝑞, �̇�𝑞)𝑣𝑣 − �̅�𝐺(𝑞𝑞) − 𝜏𝜏�̅�𝑑 + 𝐸𝐸�(𝑞𝑞)𝜏𝜏]

(5.53)

5.5 Summary

In this chapter, a complete Lagrange dynamic model of the UGV and the nonholonomic MM of

interest (used in the research) were presented. The null state space matrix 𝑆𝑆(𝑞𝑞) obtained for the

UGV and the MM was able to be used to eliminate the Lagrange multiplier to enable a simplified

(reduced, yet complete) MM dynamical model containing the constraints describing the total

energy of the system. This will enable to generate the state-space control equations capable of

controlling the MM without the use of the typical assumptions made in robotics. The dynamic

properties of the dynamic model was analyzed and verified (via diverse simulation tests in

MATLAB).

The UGV platform under development has two actuator torques but it has three DOF while the

robotic arm mounted ion the UGV has three joints and moves in 3 DOF. With respect to the

nonholonomic MM, the wheel and arm dynamics where combined together in one state space

63

control system of equations. The developed model included all dynamic constraints without any

linearization of any aspects of the MM. The results of the proposed model can be easily extended

to other MMs with reduced or increased complexities (e.g., greater DOF in the robot arm or other

UGV type of rovers).

64

CHAPTER SIX: SIMULATION FRAMEWORK

Following the kinematic and dynamic models of the MM that relaxes a number of assumptions

that have been typically used in robotics, this chapter moves on using such equations to simulate

and show their use in the control of MMs working in cooperation.

6.1 Model of the mobile manipulator using simmechanics

To simulate and analyze the MM of interest (ANA-Bot) herein the SIMMECHANICS

software is used. SIMMECHANICS software is a block diagram modeling environment for the

engineering design and simulation of rigid multibody machines and their motions, using the

dynamics of forces and torques. It was decided to use such environment due to its capabilities to

model multibody systems in a block diagram format while taking into account their mass

properties, their possible motions, and kinematic constraints and measure body motions.

As described in Chapter 4 ANA-Bot consists of a nonholonomic UGV and a robotic arm

having five fully actuated revolute joints. As a result the end gripper of the robot arm is configured

to work and be controlled in 6D included in its position (x, y and z) and orientation (yaw, pitch

and roll) (Figure 6.1).

The model presented in Figure 6.1 comprises a set of connected blocks (i.e., body, joint,

actuator, sensors, constraint, and initial conditions). These blocks were defined using special forms

available in the modeling environment and following the mathematical equations derived in

Chapters 4 and 5. All bocks included the real (experimental) MM and its properties such as mass

and moments of inertia are also included in the SIMMECHANICS representation. In Fig.6.1 seven

robot blocks are used to represent and analyze the MM (e.g., Shoulder, Waist, Wheel1, Wheel2,

etc.). Each block has a sensor set attached to it used to measure the corresponding angular position,

65

velocity and joint torques. For kinematics and dynamic simulation, motion is given to the arm

joints and wheels by joint actuator (e.g., Body 1A, Body 2, etc.). The SIMMECHANICS

representation included the dynamic and other constraints that are typically neglected as they tend

to generate, although more realistic, more complex models that present diverse challenges in

control.

Figure 6.1: Mobile Manipulator (ANA-Bot) SIMMECHANICS model.

66

In order to test the SIMMECHANICS as well as the mathematical models the schematic

representation shown in Figure 6.1 was extended to include two MMs working cooperatively

handling a rigid object. Such model, at a higher level within the SIMMECHANICS framework,

includes two MM models (left and right side blocks in Fig. 6.2, respectively) as illustrated in Figure

6.2.

Figure 6.2: Two MMs cooperating SIMMECHANIC model.

The idea behind using two MMs is to test the proposed control scheme by having one MM act as

the leader (i.e., generate a desired trajectory in space with its UGV + end effector) while the 2nd

67

MM would act as the follower (attempting to track the trajectory generated by the leader with its

own UGV + end effector system). The control architecture developed for such purpose is shown

in Fig 6.2 and described in the following section.

6.2 Control architecture

In order to address the problems found in current control mechanisms of cooperation

between two MMs a new yet simple control approach is proposed herein. The approach combines

well known techniques in an attempt to maximize their advantages while reducing their

disadvantages [59] for the intended research goal. In this architecture two MMs cooperate for

handling a rigid object in 3D. The end effectors of the MMs are assumed to be rigidly attached to

a rigid object that is being manipulated in cooperation. That is, there is no relative motion between

the end effectors and the object as typically assumed in robotics [e.g., 69]. For this, we consider

the MMs as a coupled system carrying an object (assumed to be rigid so that the object cannot

deform under any forces and thus be a means for poor indirect communication between the MMs)

in cooperation. That is there is no direct communication (e.g., wireless, centralized control, etc)

between the MMs.

In simple terms and plain language the control architecture used to test the mathematical

modeling of Chapters 4 and 5 is described next. The process starts by initializing the follower MM

system. This is achieved by defining the current interaction forces sensed between the two MMs

though the handled object as the desired force/torque interaction. That is, once the motion of the

leader starts the sensed changes in the force torque (F/T) sensor values should be equal to zero.

Subsequently, during the cooperation task there will inevitable changes sensed in the F/T values

68

due to the motion of the leading MM which will push, pull and/or torque the handled object as it

moves. According to the value of such changes, Δ (F/T), the following MM will compute its

motion by using the resolved motion rate control method including using of pseudo inverse

Jacobian to avoid reaching any singularity of its robot’s arm (i.e., joint angle velocities) as it

attempt to follow the leader. That is, the goal of the follower MM is to eliminate any changes in

the F/T values (i.e., make Δ (F/T) = 0 at all times during its motion as it follows the leader via the

motion of the handled object). Herein, we refer to this approach as a velocity feedback mechanism.

During the control effort at all times the slave MM computes the manipulability value of its robotic

arm and guides the UGV to move in a way to enable the arm to have the greatest opportunity to

follow the potentially unknown motion of the leader via the sensed object forces. The controller(s)

will send the signal commands which include the robot’s arm joints velocities and the wheel

velocities to both MM’s at the same time. The motion of the mobile base is subjected to

nonholonomic kinematics constraints, which provides the control of MMs very challenging,

especially when robots work in non-engineered environments. Figure 6.3 shows a detailed flow

chart illustrating the developed control algorithm.

In order to enable the follower MM to perform 100% of the object`s manipulation task

while leaving the leading MM to lead during the initialization process the object to be handled in

cooperation is placed on the follower robot’s gripper which in turn gathers all sensor data (at this

point the following robot holds the object by itself).

69

Figure 6.3: Flowchart of the control algorithm.

70

6.3 Simulation and results

As described above the kinematics-dynamics simulation model of the two MM swarm was

performed using SIMMECHANICS and the controller was implemented in SIMULINK. Figure

6.4 shows the results of one of the numerous tests conducted showing the performance of the initial

controller. In this test the MMs were placed on a flat terrain while handling a rigid (beam like)

object and having the leader move forward in a straight line while having its end-effector

performed a sinusoidal trajectory. Figure 6.4 shows the corresponding change in the joint arm

positions where q1 (shoulder pitch), q2 (shoulder yaw), q3 (elbow), q4 (wrist), and q5 (hand) are

the five joints in the robot arm. Figure 6.5 shows the joint velocity of the MM to compensate the

sensed changes in the F/T values.

Figure 6.4: Joint position of the MM(i), i=1,2.

71

Figure 6.5: Joint velocity of the MM (i), i=1, 2.

In this case there are two types of cooperation being considered. The first is the cooperation

between the MMs handling the object. The second is the cooperation between the follower`s arm

and mobile base to achieve the desired object’s motion (dictated by the leader) and minimize the

error of the desired trajectory. Figure 6.6 shows the UGV`s and robot arm`s linear velocity as

executed during the cooperating task. As seen in Fig. 6.6, the end-effector converges (i.e., tracks)

the desired motion accurately. The UGV also tracks the desired motion accurately with only small

disturbances due to the oscillating motion of the end-effector.

72

Figure 6.6: Vehicle’s and arm’s linear velocities (i), i=1,2.

As expected the UGV contributes to the cooperative motion in two dimensions (i.e., x and

y velocities) while the arm contributes in 3D dimensions (i.e., x, y and z velocities). That is, the

UGV + arm cooperate to tracks the motion in 2D while the arm is the only entity responsible for

the motion tracking in the vertical (z) direction. When the UGV moves on a rough terrain then the

arm is the only responsible for coping with the associated terrain disturbances which the UGV

cannot resolve.

In this approach the manipulability of the arm is used to coordinate the motion between locomotion

and manipulation of the MMs. The control mechanism continuously measures the manipulability

value and according to any computed change guides the control effort to divide the desired velocity

between the mobile base and the manipulator. Figure 6.7 shows the changes in the robot`s arm

manipulability value. As can be seen in Fig. 6.7, when the manipulability value is high the robot’s

arm velocity is maximum and when the manipulability value is minimum the vehicle’s velocity is

73

maximum. During the cooperation between the MMs the control mechanism calculates the

manipulability value and divides it by the maximum manipulability value. Subsequently, the

obtained value is multiplied by the total desired velocity to calculate the robot`s arm velocity while

the remaining of the desired object’s velocity is achieved by the vehicle (UGV). In turn, the

resolved motion rate control is used to find the desired joint velocities. Figure 6.8 shows a graphical

representation of the simulation results of the MMs cooperation in 3D environment during the

cooperation at 0, 13 and 50 seconds of the cooperation task.

Figure 6.7: Changes of the arm’s manipulability (i), i=1, 2.

74

Figure 6.8: Graphical time lapsed simulation results of two MM handling object

Similar results as the ones illustrated in Fig. 6.8 were obtained when commanding the follower to

follow the leader performing diverse motions.

6.4 Model of the mobile manipulator using simulink

During the simulation of cooperative MMs it was observed that using a 5 DOF robot arm does not

add significance and simulating a MM having a less complex robot arm is sufficient to develop

the controller set at the start of this research work (Chapter 3). In this section the simulation of the

mathematical model of the UGV and MMs system having an arm with reduced DOF is discussed.

Then in Chapter 7 the proposed controller for MM deployed in outdoor rough terrains is described.

The simplified MM consist of a UGV plus a 3 revolute joint arm. This model uses the full dynamic

and kinematic models described in Chapters 4 and 5 and as a result its implementation in

75

SIMMECHANICS follows the same architecture presented in Section 6.1. This complete

simulation is then used in the control of the given MM(s). The corresponding control architecture

will be implemented in MATLAB/SIMULINK using the whole nonholonomic constraints

(presented in Chapter 7). In the control architecture chapter (Chapter 7) a control routine and

trajectory planner blocks will be added to the MATLAB/SIMULINK model. Tests will be

performed by giving different 3D desired trajectories for the end effector to follow. In this chapter,

before a complete control system is developed and described in Chapter 7, we will test the

simulation framework to verify that the models and independent tracking problems of the

individual UGV and robot arm acting independently (no coupling) are accurate. The trajectories

used for such cases are basic but typically challenging

Figures 6.9 and 6.10 shows the developed MATLAB/Simulink model used to test the above

mentioned aspects.

76

Figure 6.9: MATLAB/Simulink model of the MMs

77

Figure 6.10: MATLAB/SIMULINK model of the kinematics, jacobian and dynamics of the

MMs

78

The nonholonomic constraint dynamic block used in the MATLAB code serves as a

complete model of the robot using the constrained Lagrange dynamics. The inputs and outputs of

such block are seen in the MATLAB function block in Simulink in Figure 6.9 and 6.10.

The SIMULINK frameworks of Figs. 6.9 and 6.10 include three other MATLAB embedded

functions, namely:

1. Embedded MATLAB function to express the dynamic of the nonholonomic MMs.

2. Embedded MATLAB function to express the kinematic of the nonholonomic MMs.

3. Embedded MATLAB function to express the Jacobian of the nonholonomic MMs.

With the above MATLAB/SIMULINK modules diverse tests were performed to verify the

accuracy of the simulation. The test ranged from tracking a circular path (Fig. 6.11), arc (Fig.

6.12) and moving forward. The end-effector`s trajectory of the mobile manipulator was also

simulated in 3D (Fig. 6.13). In all test the UGV and robot arm performed as expected.

Figure 6.11: Circular Trajectory of the mobile base in the robot`s x-y plane.

79

Figure 6.12: Arc Trajectory of the mobile base in the x-y plane.

Figure 6.13: End-Effector`s Trajectory of the MM in the x-y-z plane.

It must be notes that in all simulations the robots (UGV, robot arm or MM) accurately tracked the

desired path despite the dynamic uncertainties included.

80

CHAPTER SEVEN: TRAJECTORY TRACKING CONTROL OF MM

7.1 Introduction

In this chapter two fully developed control methods for MMs are described in detail: i) a

backstepping sliding mode CMAC, and ii) a sliding mode CMAC. The first control architecture,

a backstepping sliding mode CMAC neural network control, is developed to control the UGV

with its nonholonomic constraint. In this architecture the full dynamic and kinematic models

presented in Chapters 4 and 5 are used. The second control architecture, a sliding mode CMAC

neural network control, was developed to deal with the full MM system (UGV + arm). The goal

of this control is to enable the end-effector of the MM be the only the interacting point with the

environment including any entity that the MM is required to assist. The overall goal of the

developed architecture is for the end-effector to follow a given (a priori unknown) reference

trajectory in 3D generated by a third independent party (e.g., human, robot, etc.) via the

cooperation between the UGV and the arm comprising the MM. Simulation results for both

control developments will show the effectiveness of the proposed controller in Chapter 8.

7.2 Robust stable adaptive sliding mode backstepping neural network control of a UGV

The proposed control architecture developed for the UGV part of the MM was introduced in

Chapter 3 (Proposed work), and shown in Figure 3.4. The details of such control approach is shown

in detail in Figure 7.1. The control architecture follows the sequence described in Chapter 3. The

control employs a Lyapunov function and a CMAC neural network. These control techniques work

together to estimate the required input torque to the UGV`s tracks so that it can follow the desired

reference trajectory.

81

Figure 7.1: Control architecture of UGV

The reference trajectory tracking problem for a nonholonomic UGV is defined and formulated in

this thesis as:

�̇�𝑥𝑐𝑐𝑚𝑚𝑟𝑟𝑐𝑐 = 𝑣𝑣𝑟𝑟𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞𝑟𝑟 (7.1)

�̇�𝑦𝑐𝑐𝑚𝑚𝑟𝑟𝑐𝑐 = 𝑣𝑣𝑟𝑟𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞𝑟𝑟 (7.2)

where �̇�𝑞𝑟𝑟 = 𝑞𝑞𝑑𝑑𝑑𝑑𝑐𝑐−𝑟𝑟

The global coordinate frame of the UGV is defined by Equation (7.4) as:

(7.3)

𝜀𝜀𝑐𝑐𝑚𝑚𝑟𝑟𝑐𝑐 = [𝑥𝑥𝑟𝑟 𝑦𝑦𝑟𝑟 𝑞𝑞𝑟𝑟]𝑇𝑇 (7.4)

𝑣𝑣𝑐𝑐𝑚𝑚𝑟𝑟𝑐𝑐 = [𝑣𝑣𝑟𝑟 �̇�𝑞𝑟𝑟]𝑇𝑇 (7.5)

where vcart is a vector containing the linear and angular velocities of the UGV.

82

The backstepping control of UGVs has been employed by many researchers [61]–[63]. However,

limited work has been done for MMs working in unstructured environments. The UGV`s error

between the reference and output trajectories is converted by a coordinate transformation matrix,

Equation (7.6).

𝑒𝑒 = �𝑒𝑒1𝑒𝑒2𝑒𝑒3� = 𝐸𝐸𝑒𝑒(𝑞𝑞𝑑𝑑 − 𝑞𝑞)

�𝑒𝑒1𝑒𝑒2𝑒𝑒3� = �

𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 0−𝑠𝑠𝑖𝑖𝑎𝑎𝑞𝑞 𝑐𝑐𝑐𝑐𝑠𝑠𝑞𝑞 0

0 0 1� �𝑥𝑥�𝑦𝑦�𝑞𝑞��

(7.6)

The mechanism then computes the desired linear and angular velocities of the robot as shown in

Equation (4.7) where 𝐾𝐾1,𝐾𝐾2 and 𝐾𝐾3 are positive gains to the backstepping control.

𝑣𝑣𝑑𝑑 = � 𝑣𝑣𝑐𝑐𝑚𝑚𝑟𝑟𝑐𝑐𝑐𝑐𝑐𝑐𝑠𝑠𝑒𝑒3 + 𝐾𝐾1𝑒𝑒1�̇�𝑞𝑟𝑟 + 𝐾𝐾2𝑣𝑣𝑐𝑐𝑚𝑚𝑟𝑟𝑐𝑐𝑒𝑒2 + 𝐾𝐾3𝑣𝑣𝑐𝑐𝑚𝑚𝑟𝑟𝑐𝑐𝑠𝑠𝑖𝑖𝑎𝑎𝑒𝑒3

� (7.7)

In order to convert the error in the angular and linear velocities to a control input to be sent to the

UGV in this thesis the sliding mode control is employed.

In what follows in this section the proposed control law and the corresponding sliding mode

control with a Lyapunov function and a CMAC is derived. The error between the linear and

angular velocity is computed using Equation 7.8.

𝑒𝑒𝑣𝑣,𝑤𝑤 = 𝑒𝑒𝑣𝑣,𝑤𝑤 𝑑𝑑𝑒𝑒𝑑𝑑 − 𝑒𝑒𝑣𝑣,𝑤𝑤 (7.8)

where 𝑒𝑒𝑣𝑣,𝑤𝑤 𝑑𝑑𝑒𝑒𝑑𝑑and 𝑒𝑒𝑣𝑣,𝑤𝑤are the desired and the obtained velocities.

The derivatives of the error can now be computed as:

�̇�𝑒𝑣𝑣,𝑤𝑤 = �̇�𝑒𝑣𝑣,𝑤𝑤 𝑑𝑑𝑒𝑒𝑑𝑑 − �̇�𝑒𝑣𝑣,𝑤𝑤 (7.9)

�̈�𝑒𝑣𝑣,𝑤𝑤 = �̈�𝑒𝑣𝑣,𝑤𝑤 𝑑𝑑𝑒𝑒𝑑𝑑 − �̈�𝑒𝑣𝑣,𝑤𝑤 (7.10)

From Chapters 4 and 5 the UGV`s dynamic model control equation are:

83


�̇�𝑣 = �̈�𝑞 = 𝑀𝑀�(𝑞𝑞)−1[−𝐶𝐶̅(𝑞𝑞, �̇�𝑞)𝑣𝑣 − �̅�𝐺(𝑞𝑞) − 𝜏𝜏�̅�𝑑 + 𝐸𝐸�(𝑞𝑞)𝜏𝜏] (7.11)

In terms of the Lyapunov function an effective candidate function, V, was derived and shown in

Equation (7.14).

𝑉𝑉 =𝑠𝑠𝑇𝑇𝑀𝑀�𝑠𝑠

2+

12𝑤𝑤�𝑇𝑇Γ 𝑤𝑤� > 0

(7.14)

where Γ (the learning rate of the CMAC neural network) is defined as:

Γ = �

𝛾𝛾1 0 0 00 𝛾𝛾2 0 000

00

𝛾𝛾30

0𝛾𝛾4

�, 𝛾𝛾𝑖𝑖 > 0, 𝑖𝑖 = 1,2, . .𝑎𝑎

Such Lyapunov function includes the inertia matrix, 𝑀𝑀� , the sliding mode control, 𝑠𝑠, the weight

update rule, 𝑤𝑤� = 𝑤𝑤 − 𝑤𝑤� , and the learning rate for the CMAC, Γ. The Lyapunov candidate function

was derived to check and satisfy the stability requirement of the system. Lyapunov is tested by

ensuring that the error is not propagating while the sliding mode control effort to guarantee the

robustness of the system, especially when dealing with internal and external disturbances on/off

the system. In the proposed approach a CMAC neural network is used to approximate the unknown

dynamics and model inaccuracies which may result from unknown system parameters not possible

to obtain easily or parameters that might vary over time (e.g., suspension characteristics). The

friction between the ground and the wheel/tracks/etc. of the UGV are one of the main reason

modeled uncertainties was included in the MM model. Furthermore, another important uncertainty

includes the friction between the joint actuators in both, the tracks/wheels and robot arm joints of

the MM.

Robust control and adaptive control are two common used control techniques that deal with model

uncertainties and dynamic disturbances. However, their use has been shown to be problematic in

84

previous research. The proposed backstepping sliding mode CMAC neural network control

approach used in this thesis is hypothesized as a strong robust adaptive control technique to use

the learning abilities of the CMAC neural network and the robustness of the sliding mode control

with unmodeled dynamics and uncertainties.

CMAC it’s a lookup table nonlinear method which generates a mapping between the inputs and

the outputs. The CMAC architecture was developed by Albus James in 1975 [64]–[66] and since

then it has been modified and enhanced by numerous researchers. One of the main advantages for

selecting CMAC in the control of MMs, compared to a more traditional Multi-Layer Perceptron

(MLP) approximation technique, is that it runs very fast, which makes it very appropriate for

adaptive control systems in real time. The training of CMAC maps a set of desired control inputs

to control outputs and automatically chooses the weights of the network so that the global mapping

fits the set. In this thesis the error training method was selected and adjusted for the specific MM

at hand. The selected parameters used in this thesis of the CMAC, which were also used for the

corresponding online and offline training, are shown in Table 7.1.

The sliding mode control selected is defined by Equation (7.12):

𝑠𝑠 = 𝑐𝑐𝑒𝑒𝑣𝑣,𝑤𝑤 + �̇�𝑒𝑣𝑣,𝑤𝑤 (7.12)

where 𝑐𝑐 is a positive diagonal matrix having the following characteristic, 𝑐𝑐 = 𝑐𝑐𝑇𝑇 > 0.

The matrix 𝑐𝑐 affects the behaviour of the sliding mode controller and the control response is better

when the sliding surface parameter, s, is 𝑠𝑠 = 0 or close to the zero value.

𝑐𝑐 = �𝑐𝑐1 00 𝑐𝑐2

�, 𝑐𝑐𝑖𝑖 > 0, 𝑖𝑖 = 1,2.

�̇�𝑠 = 𝑐𝑐�̇�𝑒𝑣𝑣,𝑤𝑤 + �̈�𝑒𝑣𝑣,𝑤𝑤 (7.13)

85

Table 7.1: The parameters of CMAC for training the dynamics of UGV

Parameters of CMAC for UGV training Value

Learning rate ( Γ ) 0.2

Decay rate 0.001

No of divisions 6

Hash size 5000

No of layers 150

No of inputs for the UGV 4

No of outputs for the UGV 2

Differentiating the Lyapunov function (Eqn. 7.14) the following equation is obtained:

�̇�𝑉 = 𝑠𝑠𝑇𝑇𝑀𝑀��̇�𝑠 +𝑠𝑠𝑇𝑇𝑀𝑀� ̇ 𝑠𝑠

2+

12𝑤𝑤�𝑇𝑇 Γ 𝑤𝑤�̇

(7.15)

Substituting (7.9) and (7.10) and (7.11) into Equation (7.15) Equation (7.16) is obtained.

�̇�𝑉 = 𝑠𝑠𝑇𝑇 �𝑀𝑀�(𝑞𝑞)𝑐𝑐�̇�𝑒 + 𝑀𝑀�(𝑞𝑞)(�̈�𝑒𝑣𝑣,𝑤𝑤 𝑑𝑑𝑒𝑒𝑑𝑑 − �̈�𝑒𝑣𝑣,𝑤𝑤) +𝑀𝑀� ̇ 𝑠𝑠

2� +

12𝑤𝑤�𝑇𝑇Γ 𝑤𝑤�̇

(7.16)

Substituting Equation (7.10) into (7.16) results in:

86

�̇�𝑉 = 𝑠𝑠𝑇𝑇 �𝑀𝑀�(𝑞𝑞)𝑐𝑐�̇�𝑒𝑣𝑣,𝑤𝑤 + 𝑀𝑀�(𝑞𝑞)�̈�𝑒𝑣𝑣,𝑤𝑤 𝑑𝑑𝑒𝑒𝑑𝑑

+ 𝑀𝑀�(𝑞𝑞)(𝑀𝑀�(𝑞𝑞)−1[𝐶𝐶̅(𝑞𝑞, �̇�𝑞)𝑣𝑣 + �̅�𝐺(𝑞𝑞) + 𝜏𝜏�̅�𝑑 − 𝐸𝐸�(𝑞𝑞)𝜏𝜏])) +𝑀𝑀� ̇ (𝑞𝑞)𝑠𝑠

2�

+ 𝑤𝑤�𝑇𝑇Γ𝑤𝑤�̇

(7.17)

Then the control input 𝜏𝜏 is chosen to be:

𝜏𝜏 = 𝐸𝐸�(𝑞𝑞)−1 ��𝐶𝐶̅̂(𝑞𝑞, �̇�𝑞)𝑣𝑣 + �̅�𝐺�(𝑞𝑞) + 𝑀𝑀��(𝑞𝑞)�̈�𝑒𝑣𝑣,𝑤𝑤 𝑑𝑑𝑒𝑒𝑑𝑑 +𝑀𝑀��̇(𝑞𝑞)𝑠𝑠

2� + 𝑘𝑘𝑠𝑠� (7.18)

where 𝑀𝑀��(𝑞𝑞) is the approximation of the inertia matrix, 𝐶𝐶̅̂(𝑞𝑞, �̇�𝑞) is the approximation of the Coriolis

and Centripetal matrix, 𝐹𝐹��(𝑞𝑞) is the approximation of the friction force, and �̅�𝐺�(𝑞𝑞) is the

approximation of gravity vector.

Defining the function 𝑓𝑓 (to represent the unknown dynamics of the UGV as:

𝑓𝑓 = 𝐶𝐶̅(𝑞𝑞, �̇�𝑞) + 𝐹𝐹�(𝑞𝑞) + �̅�𝐺(𝑞𝑞) − 𝐸𝐸�(𝑞𝑞)𝜏𝜏 +𝑀𝑀� ̇ (𝑞𝑞)𝑠𝑠

2 (7.19)

Since the unknown function 𝑓𝑓 exists in the UGV the CMAC function approximation technique is

used to provide an accurate approximate to the dynamics of the UGV.

The approximation of 𝑓𝑓, 𝑓𝑓, developed within the context of this thesis is:

𝑓𝑓 = �𝐶𝐶̅̂(𝑞𝑞, �̇�𝑞)𝑣𝑣 + �̅�𝐺�(𝑞𝑞) − 𝐸𝐸��(𝑞𝑞)𝜏𝜏 + 𝑀𝑀��(𝑞𝑞)�̈�𝑒𝑣𝑣,𝑤𝑤 𝑑𝑑𝑒𝑒𝑑𝑑 +𝑀𝑀��̇(𝑞𝑞)𝑠𝑠

2� (7.20)

as defined by:

𝑓𝑓 = 𝜑𝜑(𝑥𝑥)𝑤𝑤�𝑇𝑇 (7.21)

87

where 𝑤𝑤 is the weights set in the CMAC controller, and 𝜑𝜑 is the corresponding set of shape

functions. The CMAC approximation error 𝑎𝑎 is defined as:

𝑎𝑎(�̇�𝑥, �̈�𝑥, 𝑞𝑞, �̇�𝑞) = 𝑓𝑓 − 𝜑𝜑𝑤𝑤 (7.22)

Rearranging Equation (4.22) results in Equation (4.23):

𝑓𝑓 = 𝑎𝑎(�̇�𝑥, �̈�𝑥, 𝑞𝑞, �̇�𝑞) + 𝜑𝜑𝑤𝑤 (7.23)

Substituting Equation (4.23) into (4.18) results in:

𝜏𝜏 = 𝐸𝐸�(𝑞𝑞)−1�(𝑓𝑓 + 𝑘𝑘𝑠𝑠)� (7.24)

And substituting Equations (7.24), (7.21) and (7.23) into Equation (7.17) generates the derivative

of the Lyapunov function:

�̇�𝑉 = 𝑠𝑠𝑇𝑇(𝜑𝜑𝑤𝑤 + 𝑎𝑎(𝑠𝑠) + 𝑘𝑘𝑠𝑠) + �𝑤𝑤�𝑇𝑇Γ 𝑤𝑤�̇� (7.25)

Choosing the weight update law of the proposed controller as:

𝑤𝑤�̇ = −Γ𝜑𝜑𝑠𝑠 (7.26)

the following expression is obtained:

�̇�𝑉 = 𝑠𝑠𝑇𝑇𝑎𝑎(𝑠𝑠) − 𝑠𝑠𝑇𝑇𝑘𝑘𝑠𝑠 + �𝑤𝑤�𝑇𝑇Γ 𝑤𝑤�̇� = 𝑠𝑠𝑇𝑇{𝑎𝑎(𝑠𝑠) − 𝑘𝑘𝑠𝑠} ≤ ‖𝑠𝑠‖{𝑎𝑎𝑚𝑚𝑚𝑚𝑚𝑚 − 𝐾𝐾𝑚𝑚𝑖𝑖𝑚𝑚‖𝑠𝑠‖} (7.27)

For �̇�𝑉 to remain negative definite Equation (4.28) must be satisfied where dmax and Kmin is

appositive diagonal matrix.

‖𝑠𝑠‖ >𝑎𝑎𝑚𝑚𝑚𝑚𝑚𝑚𝐾𝐾𝑚𝑚𝑖𝑖𝑚𝑚

(7.28)

When ‖𝑠𝑠‖ < 𝑎𝑎𝑚𝑚𝑚𝑚𝑚𝑚 the CMAC weights can go to infinity. To prevent this from occurring the

following control law is selected:

𝜏𝜏 = 𝐸𝐸�(𝑞𝑞)−1 �−(𝜑𝜑𝑤𝑤� − 𝑘𝑘𝑠𝑠) − 𝑎𝑎𝑚𝑚𝑚𝑚𝑚𝑚𝑠𝑠‖𝑠𝑠‖

� (7.29)

Then the control input is defined as:

88

𝜏𝜏 = 𝐸𝐸�(𝑞𝑞)−1 �𝑠𝑠𝑇𝑇{𝑎𝑎(𝑠𝑠) − 𝑘𝑘𝑠𝑠} −𝑎𝑎𝑚𝑚𝑚𝑚𝑚𝑚𝐾𝐾𝑚𝑚𝑖𝑖𝑚𝑚

� (7.30)

and �̇�𝑉 takes the following from:

�̇�𝑉 ≤ ‖𝑠𝑠‖2𝐾𝐾𝑚𝑚𝑖𝑖𝑚𝑚 (7.31)

Equation (4.31) is a negative semi definite function and as a result the controller is uniformly

ultimately bounded.

The control law used for the UGV (Equation 7.30) has three parts. The first part, (𝑘𝑘𝑠𝑠), is the sliding

mode controller. This aspect is responsible on achieving and guaranteeing the stability of the

controller as well as satisfy the ultimately bounded performance. The second part, (𝜑𝜑𝑤𝑤�), represents

the adaptive CMAC neural network having the goal to cope with the dynamic uncertainties. And

the third and last part of the controller, (�̂�𝑎𝑚𝑚𝑚𝑚𝑚𝑚𝑑𝑑‖𝑑𝑑‖

), is the robust term that attenuates the disturbances

encountered by the UGV.

7.3 Robust sliding mode CMAC control of a mobile manipulator

Now that the controller for the UGV has been developed (Section 7.2) we use such work to develop

a controller for the MM of interest. The proposed control architecture is illustrated in the flow

chart of Figure 7.2. The goal of such architecture is to ultimately compute the control torque to

control the mobile manipulator so that the error between the joint position vector 𝑞𝑞 and the desired

joint position 𝑞𝑞𝑑𝑑𝑒𝑒𝑑𝑑 can decay to zero within finite time. Thus, the selected control method

envisioned to provide such solution is the robust adaptive sliding-mode control scheme employing

a cerebellar model articulation controller (CMAC) neural network. One of the goals of such

controller to estimate the unknown function of the MM’s control system, so that the stability of

the control system can be guaranteed.

89

Figure 7.2: Control architecture of MMs

For that Equation (7.32) is utilized:


�̇�𝑣 = �̈�𝑞 = 𝑀𝑀�(𝑞𝑞)−1[−𝐶𝐶̅(𝑞𝑞, �̇�𝑞)𝑣𝑣 − �̅�𝐺(𝑞𝑞) − 𝐹𝐹�(𝑞𝑞) − 𝜏𝜏�̅�𝑑 + 𝐸𝐸�(𝑞𝑞)𝜏𝜏] (7.32)

where

𝑞𝑞 = [𝑣𝑣, �̇�𝑞, 𝑞𝑞1, 𝑞𝑞2, 𝑞𝑞3]𝑇𝑇, 𝜏𝜏 = [𝜏𝜏𝑟𝑟 , 𝜏𝜏𝑙𝑙, 𝜏𝜏1, 𝜏𝜏2, 𝜏𝜏3]𝑇𝑇, 𝑞𝑞 ∈ 𝑅𝑅𝑚𝑚 is a single vector, 𝑀𝑀�(𝑞𝑞) ∈ 𝑅𝑅𝑚𝑚×𝑚𝑚 is the

inertia matrix, 𝐶𝐶̅(𝑞𝑞, �̇�𝑞) ∈ 𝑅𝑅𝑚𝑚×𝑚𝑚 is the centrifugal and Coriolis forces, �̅�𝐺(𝑞𝑞) ∈ 𝑅𝑅𝑚𝑚 is the gravity

vector, 𝐹𝐹�(𝑞𝑞) ∈ 𝑅𝑅𝑚𝑚 is the frictional force, 𝜏𝜏�̅�𝑑 ∈ 𝑅𝑅𝑚𝑚 is the disturbance moment, 𝐸𝐸�(𝑞𝑞) ∈ 𝑅𝑅𝑚𝑚×𝑚𝑚 is a

nonsingular transformation matrix, and 𝜏𝜏 ∈ 𝑅𝑅𝑚𝑚 is the required control torque.

90

Characteristics of the dynamic model of the MM:

As described in Chapter 5 the dynamic model of the MM include certain properties repeated below

for completeness and other properties (4 to 7) are herein included as they are relevant to the

developments that will follow:

Property 1: The inertia matrix is symmetric 𝑀𝑀(𝑞𝑞) = 𝑀𝑀(𝑞𝑞)𝑇𝑇positive definite, symmetry

matrix and bounded, there exist positive constants 𝑚𝑚𝑏𝑏 ,𝑚𝑚1,𝑚𝑚2 and 𝑚𝑚3.

Property 2: Inverse of the inertia matrix 𝑀𝑀(𝑞𝑞) is exists and its positive definite as well.

Property 3: The matrix 𝑁𝑁(𝑞𝑞, �̇�𝑞) = �̇�𝑀(𝑞𝑞) − 2𝐶𝐶(𝑞𝑞, �̇�𝑞) is a skew matrix (bounded),

𝑁𝑁(𝑞𝑞, �̇�𝑞)𝑖𝑖𝑖𝑖𝑇𝑇 = −𝑁𝑁(𝑞𝑞, �̇�𝑞)𝑖𝑖𝑖𝑖.

Property 4: The skew symmetry matrix when 𝑁𝑁(𝑞𝑞, �̇�𝑞) = �̇�𝑀(𝑞𝑞) − 2𝐶𝐶(𝑞𝑞, �̇�𝑞) and it is a

positive symmetry matrix 𝑀𝑀(𝑞𝑞) = 𝑀𝑀(𝑞𝑞)𝑇𝑇. When satisfied these aspects prove the

controllability and asymptotic stability of the nonholonomic mobile robot system.

Property 5: Highly nonlinearity: Each item of the dynamic equations which describes the

complete dynamics of the MM contains non-linear factors (e.g., trigonometric functions).

Property 6: High degree of coupling due to the dynamic interactions between the mobile

base and the robot arm.

Property 7: Model dynamic has a high level of uncertainty and it is time variant.

Let 𝑞𝑞𝑑𝑑𝑒𝑒𝑑𝑑 denot the desired trajectory of the end effector which the UGV + Robot arm must

follow. Under these conditions the tracking error and its derivatives needed during the controller

design is defined as:

𝑒𝑒 = 𝑞𝑞𝑑𝑑𝑒𝑒𝑑𝑑 − 𝑞𝑞 (7.33)

�̇�𝑒 = �̇�𝑞𝑑𝑑𝑒𝑒𝑑𝑑 − �̇�𝑞 (7.34)

91

�̈�𝑒 = �̈�𝑞𝑑𝑑𝑒𝑒𝑑𝑑 − �̈�𝑞 (7.35)

Using typical control solutions the sliding variable selected is defined as:

𝑠𝑠 = 𝑐𝑐𝑒𝑒 + �̇�𝑒 (7.36)

where 𝑐𝑐 is a positive diagonal matrix, 𝑐𝑐 = 𝑐𝑐𝑇𝑇 > 0 and s is an n vector defined as:

𝑐𝑐 =

⎣⎢⎢⎢⎡𝑐𝑐1 0 0 0 00000

𝑐𝑐2000

0𝑐𝑐300

00𝑐𝑐40

000𝑐𝑐5⎦⎥⎥⎥⎤, 𝑐𝑐𝑖𝑖 > 0, 𝑖𝑖 = 1,2, . .𝑎𝑎

𝑠𝑠 = [𝑠𝑠1 𝑠𝑠2 … 𝑠𝑠𝑚𝑚]𝑇𝑇

�̇�𝑠 = 𝑐𝑐�̇�𝑒 + �̈�𝑒 (7.37)

As with the UGV case (Section 7.2) and using the same Lyapunov function candidate defined in

Equation (7.14) and repeated in Equation (7.38) for completeness as:

𝑉𝑉 =𝑠𝑠𝑇𝑇𝑀𝑀𝑠𝑠

2+

12𝑤𝑤�𝑇𝑇Γ 𝑤𝑤� > 0

(7.38)

In contrasts to the UGV case for the full MM the learning rate, Γ, takes the following form:

Γ =

⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡𝛾𝛾1 0 00 𝛾𝛾2 00 0 𝛾𝛾3

0 0 0 0 0 0 00 0 0 0 0 0 00 0 0 0 0 0 0

0 0 00 0 000000

00000

00000

𝛾𝛾4 0 0 0 0 0 00 𝛾𝛾5 0 0 0 0 000000

00000

𝛾𝛾60000

0𝛾𝛾7000

0 0 00 0 0𝛾𝛾8 0 00 𝛾𝛾9 00 0 𝛾𝛾10⎦

⎥⎥⎥⎥⎥⎥⎥⎥⎤

, 𝛾𝛾𝑖𝑖 > 0, 𝑖𝑖 = 1,2, . .𝑎𝑎

The CMAC parameters used are shown in Table 7.2.

92

Table 7.2: The parameters of CMAC for training the dynamics of MM.

Parameters of CMAC for UGV training Value

Learning rate ( Γ ) 0.2

Decay rate 0.001

No of divisions 6

Hash size 50000

No of layers 150

No of input for the MM 10

No of output for the MM 5

Therefore, as before, the derivative of the candidate Lyapunov function takes the following form:

�̇�𝑉 = 𝑠𝑠𝑇𝑇𝑀𝑀��̇�𝑠 +𝑠𝑠𝑇𝑇𝑀𝑀� ̇ 𝑠𝑠

2+

12𝑤𝑤�𝑇𝑇 Γ 𝑤𝑤�̇

(7.39)

Substituting Equations (7.33) and (7.34) into (7.35), Equation (7.40) is derived:

�̇�𝑉 = 𝑠𝑠𝑇𝑇 �𝑀𝑀�(𝑞𝑞)𝑐𝑐�̇�𝑒 + 𝑀𝑀�(𝑞𝑞)(�̈�𝑞𝑑𝑑𝑒𝑒𝑑𝑑 − �̈�𝑞𝑚𝑚𝑐𝑐𝑐𝑐𝑎𝑎𝑚𝑚𝑙𝑙) +𝑀𝑀� ̇ 𝑠𝑠

2� +

12𝑤𝑤�𝑇𝑇Γ 𝑤𝑤�̇

(7.40)

Substituting Equation (7.32) into Equation (7.40) the following expression is obtained:

Then the control input 𝜏𝜏 is chosen to be:

�̇�𝑉 = 𝑠𝑠𝑇𝑇 �𝑀𝑀�(𝑞𝑞)𝑐𝑐�̇�𝑒 + 𝑀𝑀�(𝑞𝑞)�̈�𝑞𝑑𝑑𝑒𝑒𝑑𝑑

+ 𝑀𝑀�(𝑞𝑞)(𝑀𝑀�(𝑞𝑞)−1[𝐶𝐶̅(𝑞𝑞, �̇�𝑞)𝑣𝑣 + �̅�𝐺(𝑞𝑞) + 𝜏𝜏�̅�𝑑 + 𝐹𝐹�(𝑞𝑞) − 𝐸𝐸�(𝑞𝑞)𝜏𝜏]))

+𝑀𝑀� ̇ (𝑞𝑞)𝑠𝑠

2� + 𝑤𝑤�𝑇𝑇Γ𝑤𝑤�̇

(7.41)

93

𝜏𝜏 = 𝐸𝐸�(𝑞𝑞)−1 ��𝐶𝐶̅̂(𝑞𝑞, �̇�𝑞)𝑣𝑣 + 𝐹𝐹��(𝑞𝑞) + �̅�𝐺�(𝑞𝑞) + 𝑀𝑀��(𝑞𝑞)�̈�𝑞𝑑𝑑𝑒𝑒𝑑𝑑 +𝑀𝑀��̇(𝑞𝑞)𝑠𝑠

2� + 𝑘𝑘𝑠𝑠� (7.42)

where 𝑀𝑀��(𝑞𝑞), 𝐶𝐶̅̂(𝑞𝑞, �̇�𝑞), 𝐹𝐹��(𝑞𝑞), �̅�𝐺�(𝑞𝑞) are as defined in Equation (7.18).

Defining the function 𝑓𝑓 as the unknown dynamics of the MM via Equation (7.43):

𝑓𝑓 = 𝐶𝐶̅(𝑞𝑞, �̇�𝑞) + 𝐹𝐹�(𝑞𝑞) + �̅�𝐺(𝑞𝑞) − 𝐸𝐸�(𝑞𝑞)𝜏𝜏 +𝑀𝑀� ̇ (𝑞𝑞)𝑠𝑠

2 (7.43)

The approximation of 𝑓𝑓 of the unknown dynamic is described by Equation 7.44.

𝑓𝑓 = �𝐶𝐶̅̂(𝑞𝑞, �̇�𝑞)𝑣𝑣 + 𝐹𝐹��(𝑞𝑞) + �̅�𝐺�(𝑞𝑞) − 𝐸𝐸��(𝑞𝑞)𝜏𝜏 + 𝑀𝑀��(𝑞𝑞)�̈�𝑞𝑑𝑑𝑒𝑒𝑑𝑑 +𝑀𝑀��̇(𝑞𝑞)𝑠𝑠

2� (7.44)

Once again, the CMAC approximation of 𝑓𝑓 is defined by:

𝑓𝑓 = 𝜑𝜑(𝑥𝑥)𝑤𝑤�𝑇𝑇 (7.45)

where 𝑤𝑤, and 𝜑𝜑 have been defined. Similar to the UGV control case the CMAC approximation

error 𝑎𝑎 is defined as:

𝑓𝑓 = 𝑎𝑎(�̇�𝑥, �̈�𝑥, 𝑞𝑞, �̇�𝑞) + 𝜑𝜑𝑤𝑤 (7.46)

Substituting Equation (7.46) into (7.42) Equation (7.47) is obtained:

𝜏𝜏 = 𝐸𝐸�(𝑞𝑞)−1�𝑓𝑓 + 𝑘𝑘𝑠𝑠� (7.47)

Substituting Equations (7.42), (7.45) and (7.47) into (7.41) the following derivative of the

developed Lyapunov function is obtained:

�̇�𝑉 = 𝑠𝑠𝑇𝑇(𝜑𝜑𝑤𝑤 + 𝑎𝑎(𝑠𝑠) + 𝑘𝑘𝑠𝑠) + �𝑤𝑤�𝑇𝑇Γ 𝑤𝑤�̇� (7.48)

Choosing the weight update law for the CMAC as:

𝑤𝑤�̇ = −Γ𝜑𝜑𝑠𝑠 (7.49)

one obtains:

94

�̇�𝑉 = 𝑠𝑠𝑇𝑇𝑎𝑎(𝑠𝑠) − 𝑠𝑠𝑇𝑇𝑘𝑘𝑠𝑠 + �𝑤𝑤�𝑇𝑇Γ 𝑤𝑤�̇� = 𝑠𝑠𝑇𝑇{𝑎𝑎(𝑠𝑠) − 𝑘𝑘𝑠𝑠} ≤ ‖𝑠𝑠‖{𝑎𝑎𝑚𝑚𝑚𝑚𝑚𝑚 − 𝐾𝐾𝑚𝑚𝑖𝑖𝑚𝑚‖𝑠𝑠‖} (7.50)

As before, in order for �̇�𝑉 to remain negative definite and to prevent the CMAC weights to go to

infinity Equations (7.28) and (7.29) needs to be satisfied. As a result the control law described in

Equation (7.30) can also be used in this case having the same three aspects described at the end of

Section 7.2.

Substituting equation (7.29) into (7.48), Equation (7.51) is obtained

𝜏𝜏 = 𝐸𝐸�(𝑞𝑞)−1 �𝑠𝑠𝑇𝑇{𝑎𝑎(𝑠𝑠) − 𝑘𝑘𝑠𝑠} −�̂�𝑎𝑚𝑚𝑚𝑚𝑚𝑚𝐾𝐾𝑚𝑚𝑖𝑖𝑚𝑚

� (7.51)

while the following derivative of the Lyapunov candidate function:

�̇�𝑉 ≤ ‖𝑠𝑠‖2𝐾𝐾𝑚𝑚𝑖𝑖𝑚𝑚 (7.52)

which is a negative semi definite function, and as a result the controller is uniformly ultimate

bounded

7.4 Summary

In this chapter, two effective controllers, one for a UGV and another for a MM, deployed in

unstructured terrains were developed. The controllers is a combination of backstepping, sliding

mode, Lyapunov and CMAC function approximation mechanisms. An adaptive robust tracking

control strategy was presented systematically for a UGV and a MM in the existence of dynamic

uncertainties and disturbances. The proposed controllers are novel in the sense that previous

control efforts have not considered the dynamics and uncertainties as they present a number of

challenges, but these aspects have been used in the current control derivations. A CMAC controller

was used to approximate the dynamics of the system and suitable Lyapunov functions were

developed to guarantee stability and bounded of the system. In the next chapter (Chapter 8) these

95

control architectures are tested and analyzed under diverse operating conditions using the

MATLAB/SIMULINK simulation tools.

96

CHAPTER EIGHT: SIMULATION RESULTS

8.1 Introduction

In this chapter diverse test scenarios are used to test and evaluate the performance and effectiveness

of the backstepping sliding mode CMAC control mechanisms developed in Chapter 7. The

parameters of the robots (UGV and Robot arm) used in the tests/simulations are shown in Table

8.1. The tests consisted on having the robot follow a referenced trajectory in 2D or 3D space. First,

the backstepping sliding mode CMAC control proposed for the UGV will be tested by giving the

robot some traditionally difficult reference trajectories. In order to show the effectiveness of the

proposed developments the results will be compared with the results reported on previous research

using similar trajectories. Second the sliding mode CMAC control for the MMs will be showcased

by issuing commands to the end-effector of the MM to follow a predefined trajectory in 3D

Cartesian space. Such trajectory will be assumed to be a priori unknown and provided by an

unknown entity such as a human (operator), robotic arm, or another MM which are considered to

be the leader entity while the MM is considered to the follower agent.

8.2 Simulation framework and results of control of UGV

The simulation framework of the proposed controller of the UGV is shown in Figure 8.1. First the

simulation block starting by giving a reference trajectory as defined in Chapter 7: 𝑞𝑞𝑟𝑟 =

[𝑥𝑥𝑟𝑟 𝑦𝑦𝑟𝑟 𝑞𝑞𝑟𝑟]𝑇𝑇. As described in Chapter 7 the input for the backstepping controller is the error

between the reference and the actual trajectories (position) where the actual trajectory is given as

𝑞𝑞𝑚𝑚 = [𝑥𝑥 𝑦𝑦 𝑞𝑞]. Similarly, the input for the sliding mode controller, part of the UGV controller,

is the difference between the desired linear and angular velocities of the UGV. The goal from the

backstepping is to calculate the desired angular and linear velocity of the UGV to be able to achieve

97

the reference trajectory. After training the CMAC neural network in an off line manner and finding

the corresponding optimization parameters (Table 7.1) to be used in the online training the

simulations shown in the following sections are provided.

Table 8.1: Mobile Manipulator robot parameters.

Parameter Value Units

Mass of the UGV: 𝑚𝑚𝑏𝑏 50 kg

Mass of robot`s arm link 1: 𝑚𝑚1 2 kg



Radius of the wheel of the UGV: 𝑟𝑟 0.06 meter

Length of robot`s arm link 1: 𝑙𝑙1 0.06 meter



The distance between the two wheels on the UGV: 𝑏𝑏 0.3 meter

The distance between the center of the wheels and the manipulator base: 𝑙𝑙𝑏𝑏 0.2 meter

8.3 Results for the UGV control strategy:

The results shown are for two trajectories: a) a circular path, and b) a spiral curve. The actual

computed trajectories of the UGV are shown in Figures (8.2) and (8.5). From the obtained results

it can be seen that the UGV always converges to the desired trajectory rapidly and realizes a good

tracking, even in cases where the UGV is commanded to repeat the operation. Therefore the

proposed controller’s structure of the UGV is effective, adaptive, good trajectory tracking, stable

and robust to both the dynamic uncertainties and external disturbances. The disturbances used were

sinusoidal functions applied to the wheel actuators (not shown in the plots below).

98

This proposed control scheme method uses the advantages of the sliding mode control strong

robust performance, and the self-learning abilities of the CMAC to control the UGV with dynamic

uncertainties and external disturbances.

Figure 8.1: Simulation of the proposed control of UGV.

99

8.3.1 Test I: Circular path

A circular reference trajectory was used for the UGV to follow. This is another well-known

reference trajectory commonly use in testing by many researchers in the field. Figure 8.2 shows

the results of the tracking process using a Simulink and embedded MATLAB functions. The

initial position and orientation of the UGV are (x=0, y=0, q=0) while the final position and

orientation are set to (x=0, y=0, q=6.5). As seen in Figure 8.2, the UGV converges and tracks

the desired trajectory (the error converges to 0).

Figure 8.2: Simulation of the UGV in x-y plane (Circle reference trajectory).

The x and y coordinate of the trajectories of the UGV during the test are provided in Figure 8.3.

The robot starts at a 0 degree (0 radians) orientation and completes a full turn (360 degrees or

rotation) as the UGV completes the path. In all tested cases (different circular paths each having

a different radius) the error converged to zero as shown in Figure 8.4.

100

(a)

(b)

(c)

Figure 8.3: The UGV actual output states time response vs. reference trajectories of UGV: (a) x, (b) y, and (c) q.

101

(a)

(b)

(c)

Figure 8.4: The UGV actual output states error (a) ex, (b) ey, and (c) eq time response vs. reference trajectories of UGV.

102

8.3.2 Case II: Spiral reference trajectory

A spiral reference trajectory was used for the UGV to follow. Figure 8.5 shows the results of the

tracking process using a Simulink and embedded MATLAB functions. The initial position and

orientation of the UGV are (x=0, y=0, q=0) while the final position and orientation are set to (x=-

0.5, y=-2, q=6). As seen in Figure 8.5, the UGV converges and tracks the desired trajectory (the

error converges to 0).

Figure 8.5: Simulation of the UGV in x-y plane (Spiral reference trajectory).

The x and y coordinates of the trajectory of the UGV during the test are provided in Figure 8.6.

The robot starts at a 0 degree (0 radians) orientation and completes the desired spiral path with

an orientation of 340 degrees (or 6 radian) as the UGV reaches the desired goal. In some cases

(diverse spiral curves) the observed error converged to zero. However, in other cases the error

was not zero as shown in Figure 8.7 but the controller was able to handle it while coping with the

numerous (dynamic) uncertainties. The greatest error was observed to be in the vehicle’s

103

orientation (Fig. 8.7c). This was due to the fact that it was typical that the desired orientation

conflicted with the desired motion as the UGV followed the path (i.e., the orientation is a

function of the vehicle’s position).

(a)

(b)

(c)

Figure 8.6: The UGV actual output states time response vs. reference trajectories of UGV: (a) x, (b) y, and (c) q.

104

(a)

(b)

(c)

Figure 8.7: The UGV output states error (a) ex, (b) ey, and (c) eq time response vs. reference trajectories of UGV.

8.4 Simulation framework and control results of the mobile manipulator

In this section the test results when controlling the MM are presented. Two cases following a 3D

path are used to illustrate the performance of the proposed controller: a) a smooth curve (Fig. 8.9),

and b) a complex path having sharp curves (Fig. 8.16). Numerous test, not included in this

105

document were performed but not described here as the obtained results showed similar

behaviours.

The proposed simulation framework for testing the proposed control architecture for MMs is

shown in Figure 8.8. This architecture implements the control architecture described in Section

7.3. It consists of different SIMULINK embedded MATLAB function blocks. A first block (shown

in green dashed line in Fig. 8.8) represent the reference trajectory.

Figure 8.8: Simulation of the proposed control of MMs.

The second block (shown in a red dashed line in Fig. 8.8) includes the sliding mode control law,

the robust term, and the Lyapunov equation. The output from the second block is the feedback

torque used as an input to the controller. The third block (shown in a blue dashed line in Fig. 8.8)

106

is the CMAC approximation algorithm which calls a number of MATLAB functions (e.g., hash

table, initial training, CMAC adaptive function). The output from this block is the feedforward

torque. Next the disturbance blocks (shown in a green dashed line in Fig. 8.8) are applied to the

joint actuators, The last stage is sending the total torque (Feedback + feedforward) as an input for

the last block (shown as a filled cyan box in Fig. 8.8) representing the mathematical model of the

robot.

8.5: Results for the mobile manipulator control strategy

8.3.1 Case I: 3D Smooth Curve reference trajectory

For this case the end effector within the MM was initially positioned at (x=1, y=0.5, z=1). The

desired end-effector’s trajectory was set as a smooth curve in 3D Cartesian space (Fig. 8.9). During

the test the UGV and the robot arm cooperated to make possible the end effector tracked the curve.

It must be noted that the selected curve always within the configuration space of the UGV + arm

(the joint limitations are never exceeded).

The plot shown in Figure 8.9 show that the desired and obtained paths where achieved at all times,

as expected. The end-effector was able to track the desired trajectory with zero tracking error. We

thus conclude that the robust adaptive sliding mode CMAC controller of the MM is making the

system stable, adaptive while enabling effective UGV and robot arm cooperation.

It is noticeable that the tracking performance of the sliding mode CMAC neural network control

can be achieved successfully as well as providing an effective cooperation mechanism.

107

Figure 8.9: Simulation of the MM end-effector trajectory in x-y-z plane (Curve reference trajectory)

The x, y, and z coordinates of the end effector’s trajectory during the test are provided in Figure

8.10. These plots show that through UGV and arm cooperation the tracking was achieved without

errors.

(a)

Figure 8.10: The MM actual output end-effector trajectory time response vs. reference trajectories of MM: (a) x (b) y and (c) z trajectories.

108

(b)

Figure 8.10: Continued.

Figure 8.11 shows two of the three robot arm joint angles used to achieve the desired trajectory

while Figure 8.12 shoes the corresponding joint velocities.

(a)

Figure 8.11: The MM tracking performance joint positions (a) Joint 1 and (b) Joint 2.

109

(b)

Figure 8.11: Continued.

(a)

(b)

Figure 8.12: The MM tracking performance joint velocities (a) Joint 1 and (b) Joint 2.

To illustrate the result from a different perspective Figures 8.13 to 8, 15 displays the robot arm

trajectories with a top view (Fig 8.13), side view (Fig. 8.14), and front view (Fig 8.15).

110

Figure 8.13: Simulation of the MM end-effector trajectory in x-y plane (Top view).

Figure 8.14: Simulation of the MM end-effector trajectory in x-z plane (Side view),

Figure 8.15: Simulation of the MM end-effector trajectory in y-z plane (Front view).

8.3.2 Case II: 3D complex path having sharp curves

For this case the end effector was initially positioned at (x=3, y=3, z=3) and commanded to follow

a complex curve with sharp turns (Fig. 8.16). The selected curve was such that the path was

occasionally outside the configuration space of the UGV + arm (the joint limitations were

111

exceeded). This was done to test the controller under aggressive difficult to achieve trajectories.

During the test the UGV and the robot arm cooperated to make possible the end effector tracked

the curve.

The plot shown in Figure 8.16 show that the desired and obtained paths where satisfied to a certain

degree, as expected. The end-effector was not able to track the desired trajectory with zero tracking

error at all times, as expected. However, the error does not propagates even though in the situations

where the path lies outside the configuration space (where there is no possible solution). We thus

conclude that the robust adaptive sliding mode CMAC controller of the MM enables the MM

system do its best effort in completing the mission. Furthermore, the controller is stable, adaptive

while enabling effective UGV and robot arm cooperation despite impossible missions.

Figure 8.16: Simulation of the MM end-effector trajectory in x-y-z plane (Curve reference trajectory)

It is concluded that even though the curve falls beyond the capabilities of the MM, the MM does

a good job in tracking the path to the best of its abilities.

The x, y, and z coordinates of the end effector’s trajectory during the test are provided in Figure

8.17. These plots show that through UGV and arm cooperation the tracking was achieved with the

112

errors coming from the x and y directions (the error in the z direction was zero at all times). The

reason for this is the fact that the shared coordination between the UGV and the arm is only in the

x and y directions, while the tracking in the z direction is exclusively achieved by the arm (no need

of UGV and arm cooperation).

(a)

(b)

(c)

Figure 8.17: The MM output end-effector trajectory time response vs. reference trajectories of MM.

Figure 8.18 shows two of the corresponding three joint positions, while Fig 8.19 provides the

corresponding joint velocities.

113

(a)

(b)

Figure 8.18: (a and b) The MM tracking performance Joint position

The effectiveness of the cooperation mechanisms in all tests was determined via two aspects

discussed in previous chapters: i) the ability of the MM to track the desired trajectory, and ii) the

error measured in the wrist’s force/torque sensor, ∆F/T, after the initialization process. A value

of ∆F/T = 0 represents perfect cooperation.

8.6 Summary

The above complete analysis and simulation results of the backstepping sliding mode CMAC

neural network control shows that the proposed control architecture and simulation is a good

trajectory tracking controller according to the following advantages that it has over the traditional

control techniques. This control method produce zero tracking error nevertheless the shape of the

desired reference trajectory. A smooth robot trajectory will be formed using this controller

114

technique which makes it easier to implement in real life applications. This control technique is

adaptive and robust to parameters uncertainties and unknown dynamics. This control techniques

shows stability because the error never propagates.

(a)

(b)

Figure 8.19: (a and b) The MM tracking performance joint velocity

115

CHAPTER NINE: CONCLUSIONS

This thesis addressed the problem of design and implementation of a controller capable of tracking

a reference trajectory in outdoor 3D space for UGV and nonholonomic MMs in the presence of

parametric uncertainties and external disturbances. This is achieved by following the sequence of

steps outlined in this thesis (Chapter 3). This thesis achieved a simple yet effective two control

techniques with the corresponding software to enable mobile manipulator systems to interact with

the environment and cope with the associated complexities present in unstructured terrains. The

controller also enables the arm(s) to follow a given referenced trajectory despite the existence of

dynamic uncertainties and unknown parameters of the robot. A mathematical framework used to

model MM’s in Cartesian space without using any approximations was derived with the goal to

enable better control of such systems in real world unstructured environments. The properties of

the dynamic model were analyzed and verified via diverse simulation tests in MTLAB. A

controller for cooperating MMs that takes into the account the associated (possible coupled)

kinematic constraints between the MMs was developed. The result provides increased adaptability

for the coupled movement in unknown dynamic environments when unknown dynamics exist due

to parameter uncertainties and when disturbances are considered. Under this solution one of the

goals was to prove the stability of the system. This was achieved by developing a set of effective

Lyapunov functions based on previously developed concepts/functions.

A neural network Cerebellar Model Articulation Controller (CMAC) control system for

nonholonomic redundant mobile manipulators with applications to MMs was also developed. One

of the associated contributions is for this control system was to combine the end-effector trajectory

of the mobile manipulator and the trajectory of a mobile platform (UGV) in a single controller

116

(something that has been attempted before but a complete solution has not yet been found, until

now).

A redundancy resolution scheme and inverse kinematics to deal with the singularities and the robot

arm joints limits was used. This includes the use of the manipulability and Pseudo inverse Jacobian

concepts in the control architectures in the context of MM (not used by other researchers). The

purpose of such approach was to avoid online inverse kinematic calculations and the associated

singularity problems that have been problematic in the control of MMs. For this a joint space

dynamic system was derived and a similar analysis for the control of MMs cooperating in swarm

systems (2 or more MMs) was carried out.

The original set of contributions proposed in Chapter 3 were achieved. However future work still

remains to be performed.

Suggested future work can be thought of a short and long terms activities. In the short term future

work includes: i) experimental testing and validation of the proposed controllers, ii) CMAC

optimization to enable faster learning and thus coping with the uncertainties in real time, and iii)

implement and test the control architecture described in Chapter 7 to swarm MMs handling flexible

objects as opposed to only manipulating rigid objects.

The long term future research is envisioned to: i) extend the proposed controller for ground MMs

to aerial MMs and underwater MMs where the dynamic uncertainties are greater, ii) the use of

compliance based robot arms in the MMs control schemes, and iii) cooperation of heterogeneous

swarm MMs.

117

BIBLIOGRAPHY

[1] S. Lin and a a Goldenberg, “Neural-network control of mobile manipulators.,” IEEE

Trans. Neural Netw., vol. 12, no. 5, pp. 1121–33, Jan. 2001.

[2] H. Su and V. Krovi, “Decentralized dynamic control of a nonholonomic mobile

manipulator collective: a simulation study,” ASME 2008 Dyn. Syst. Control Conf. Parts A

B, Ann Arbor, Michigan, USA, pp. 661–668, 2008.

[3] Y. U. N. Y. Cao, A. S. Fukunaga, and A. B. Kahng, “Cooperative mobile robotics :

antecedents and directions,” Auton. Robots, vol. 23, no. 4, pp. 1–23, 1997.

[4] T. . A. E. Pagello, “Guest editorial advances in multirobot systems,” IEEE Trans. Robot.

Autom., vol. 18, no. 5, pp. 655–661, 2002.

[5] R. Holmberg and A. Casal, “Coordination and decentralized cooperation of multiple

mobile manipulatiors,” J. Robot. Syst., vol. 13, no. 11, pp. 755–764, 1996.

[6] T. G. Sugar and V. Kumar, “Control of cooperating mobile manipulators,” IEEE Trans.

Robot. Autom., vol. 18, no. 1, pp. 94–103, 2002.

[7] H. G. Tanner, K. J. Kyriakopoulos, and N. J. Krikelis, “Modeling of multiple mobile

manipulators handling a common deformable object,” J. Robot. Syst., vol. 15, no. 11, pp.

1–35, 1998.

[8] H. G. Tanner, S. G. Loizou, and K. J. Kyriakopoulos, “Nonholonomic navigation and

control of cooperating mobile manipulators,” IEEE Trans. Robot. Autom., vol. 19, no. 1,

pp. 53–64, Feb. 2003.

[9] H. D. N. Terry L. Huntsberger, Ashitey Trebi-Ollennu, Hrand Aghazarian, Paul S.

118

Schenker, Paolo Pirjanian, “Distributed control of multi-robot systems engaged in tightly

coupled tasks,” Auton. Robots, vol. 17, no. 1, pp. 79–92, 2004.

[10] Z. Li, S. S. Ge, and Z. Wang, “Robust adaptive control of coordinated multiple mobile

manipulators,” Mechatronics, vol. 18, no. 5–6, pp. 239–250, Jun. 2008.

[11] A. Nakamura, J. Ota, and T. Arai, “Human-supervised multiple mobile robot system,” vol.

18, no. 5, pp. 728–743, 2002.

[12] M. W. Chen and A. M. S. Zalzala, “Dynamic modelling and genetic-based trajectory

generation for non-holonomic mobile manipulators,” Control Eng. Pract., vol. 5, no. 1,

pp. 39–48, 1997.

[13] Y. Yamamoto and Y. Xiaoping, “Coordinating locomotion and manipulation of a mobile

manipulator,” Autom. Control. IEEE Trans., vol. 39, no. 6, pp. 1326–1332, 1994.

[14] M. H. Korayem, V. Azimirad, B. Tabibian, and M. Abolhasani, “Analysis and

experimental study of non-holonomic mobile manipulator in presence of obstacles for

moving boundary condition,” Acta Astronaut., vol. 67, no. 7â€“8, pp. 659–672, 2010.

[15] M. H. Korayem, M. Irani, and S. Rafee Nekoo, “Load maximization of flexible joint

mechanical manipulator using nonlinear optimal controller,” Acta Astronaut., vol. 69, no.

7â€“8, pp. 458–469, 2011.

[16] Z. Li, S. S. Ge, M. Adams, and W. S. Wijesoma, “Robust adaptive control of uncertain

force/motion constrained nonholonomic mobile manipulators,” Automatica, vol. 44, no. 3,

pp. 776–784, Mar. 2008.

[17] Y. L. * and Y. Liu, “Robust adaptive neural fuzzy control for autonomous redundant non-

119

holonomic mobile modular manipulators,” Int. J. Veh. Auton. Syst., vol. 4, no. 2, pp. 268–

284, 2006.

[18] Z. Li and W. Chen, “Adaptive neural-fuzzy control of uncertain constrained multiple

coordinated nonholonomic mobile manipulators,” Eng. Appl. Artif. Intell., vol. 21, no. 7,

pp. 985–1000, Oct. 2008.

[19] Z. Li, J. Li, and Y. Kang, “Adaptive robust coordinated control of multiple mobile

manipulators interacting with rigid environments,” Automatica, vol. 46, no. 12, pp. 2028–

2034, Dec. 2010.

[20] R. Solea and D. C. Cernega, “Sliding-mode control of coordinated nonholonomic mobile

manipulators,” System Theory, Control, and Computing (ICSTCC), 2011 15th

International Conference on. pp. 1–6, 2011.

[21] L. Ding, H. Gao, K. Xia, Z. Liu, J. Tao, and Y. Liu, “Adaptive sliding mode control of

mobile manipulators with markovian switching joints,” J. Appl. Math., vol. 2012, pp. 1–

24, 2012.

[22] M. Galicki, “Real-time constrained trajectory generation of mobile manipulators,” Rob.

Auton. Syst., vol. 78, pp. 49–62, 2016.

[23] M. Galicki, “An adaptive non-linear constraint control of mobile manipulators,” Mech.

Mach. Theory, vol. 88, pp. 63–85, 2015.

[24] M. H. Korayem, R. A. Esfeden, and S. R. Nekoo, “Path planning algorithm in wheeled

mobile manipulators based on motion of arms †,” vol. 29, no. 4, 2015.

[25] A. Mazur and K. Kozlowski, “Path following for nonholonomic mobile manipulators,”

120

Int. Symp. Robot. Res., pp. 1–16, 2015.

[26] J. Ratajczak and K. Tchoń, “Dynamically consistent Jacobian inverse for mobile

manipulators,” Int. J. Control, vol. 7179, no. March, pp. 1–10, 2015.

[27] J. O. F. Theoretical and A. Mechanics, “Payload maximization for mobile flexible

manipulators,” pp. 911–923, 2015.

[28] J. O. Jang, “Adaptive NFN nonlinearity compensation for mobile manipulators,” J. Next

Gener. Inf. Technol., vol. 4, no. 2, pp. 59–75, Apr. 2013.

[29] Y. Wang, Z. Miao, L. Liu, and Y. Chen, “Adaptive robust control of nonholonomic

mobile manipulators with an application to condenser cleaning robotic systems,” pp. 358–

363, 2013.

[30] G. Ą. K. Paj, “Planning of collision-free trajectory for mobile manipulators,” vol. 18, no.

2, pp. 475–489, 2013.

[31] A. Mazur and M. Cholewi??ski, “Virtual force concept in steering mobile manipulators

with skid-steering platform moving in unknown environment,” J. Intell. Robot. Syst.

Theory Appl., vol. 77, no. 3–4, pp. 433–443, 2013.

[32] C. Tang, C. Xu, A. Ming, and M. Shimojo, “Cooperative control of two mobile

manipulators transporting objects on the slope,” in 2009 IEEE International Conference

on Mechatronics and Automation, ICMA 2009, 2009.

[33] R. Solea and D. C. Cernega, “Sliding-mode control of coordinated nonholonomic mobile

manipulators,” 15th Int. Conf. Syst. Theory, Control. Comput., pp. 1–6, 2011.

[34] V. Andaluz, V. T. L. Rampinelli, F. Roberti, and R. Carelli, “Coordinated cooperative

121

control of mobile manipulators,” 2011 IEEE Int. Conf. Ind. Technol., pp. 300–305, Mar.

2011.

[35] C. Yang, Z. Li, and Y. Tang, “Decentralised adaptive fuzzy control of coordinated

multiple mobile manipulators interacting with non-rigid environments,” IET Control

Theory Appl., vol. 7, no. 3, pp. 397–410, Feb. 2013.

[36] L. Xiao and Y. Zhang, “A new performance index for the repetitive motion of mobile

manipulators,” IEEE Trans. Cybern., pp. 1–13, May 2013.

[37] H. Yang and D. Lee, “Cooperative grasping control of multiple mobile manipulators with

obstacle avoidance,” 2013.

[38] T. Yoshikawa, “Coordinated dynamic control for multiple robotic mechanisms handling

an object,” Proc. IROS ’91IEEE/RSJ Int. Work. Intell. Robot. Syst. '91, no. 91, pp. 315–

320, 1991.

[39] Y. Kume, Y. Hirata, K. Kosuge, H. Asama, H. Kaetsu, and K. Kawabata, “Decentralized

control of multiple mobile robots transporting a single object in coordination without

using force/torque sensors,” Proc. 2001 ICRA. IEEE Int. Conf. Robot. Autom., vol. 3, pp.

3004–3009, 2001.

[40] K. Kosuge and T. Oosumi, “Decentralized control of multiple robots handling an object,”

Proc. IEEE/RSJ Int. Conf. Intell. Robot. Syst. IROS ’96, vol. 1, pp. 318–323, 1996.

[41] G. Dai and Y. Liu, “Leaderless and leader-following consensus for networked mobile

manipulators with communication delays,” IEEE Multi-Conference Syst. Control Sept. 21-

23, 2015. Sydney, Aust., pp. 1656–1661, 2015.

122

[42] A. Dietrich, K. Bussmann, F. Petit, P. Kotyczka, C. Ott, B. Lohmann, and A. Albu-

Schäffer, “Whole-body impedance control of wheeled mobile manipulators,” Auton.

Robots, vol. 40, no. 3, pp. 505–517, 2015.

[43] T. Sugar, V. Kumar, and G. Robotics, “Decentralized control of cooperating mobile

manipulators,” Proc. 1998 IEEE Int. Conf. Robot. Autom. Leuven, Belgium, no. May,

1998.

[44] O. Khatib, K. Yokoi, K. Chang, D. Ruspini, R. Holmberg, and A. Casal, “Vehicle/Arm

coordination and multiple mobile manipulator decentralized cooperation,” Proc.

IEEE/RSJ Int. Conf. Intell. Robot. Syst. IROS ’96, vol. 2, pp. 546–553, 1996.

[45] M. Fujii and W. Inamura, “Cooperative control of multiple mobile robots transporting a

single object with loose handling,” Proc. 2007 IEEE Int. Conf. Robot. Biomimetics

December , Sanya, China, pp. 816–822, 2008.

[46] A. Petitti, A. Franchi, D. Di Paola, A. Rizzo, A. Petitti, A. Franchi, D. Di Paola, A. Rizzo,

D. Motion, A. Petitti, A. Franchi, D. Di Paola, and A. Rizzo, “Decentralized motion

control for cooperative manipulation with a team of networked mobile manipulators,”

2016.

[47] O. Madsen, S. Bøgh, C. Schou, R. S. Andersen, J. S. Damgaard, M. R. Pedersen, and V.

Krüger, “Integration of mobile manipulators in an industrial production,” Ind. Robot An

Int. J., vol. 42, no. 1, pp. 11–18, 2015.

[48] P. Lehner, “Incremental , sensor-based motion generation for mobile manipulators in

unknown , dynamic environments,” pp. 4761–4767, 2015.

123

[49] P. Falco and C. Natale, “Low-level flexible planning for mobile manipulators: a

distributed perception approach,” Adv. Robot., vol. 28, no. 21, pp. 1431–1444, 2014.

[50] T. Hekmatfar, E. Masehian, and S. J. Mousavi, “Cooperative object transportation by

multiple mobile manipulators through a hierarchical planning architecture,” 2014 2nd

RSI/ISM Int. Conf. Robot. Mechatronics, ICRoM 2014, pp. 503–508, 2014.

[51] H. Jeong, H. Kim, J. Cheong, and W. Kim, “Virtual joint method for kinematic modeling

of wheeled mobile manipulators,” Int. J. Control. Autom. Syst., vol. 12, no. 5, pp. 1059–

1069, 2014.

[52] W. Sun, W.-X. Yuan, and Y.-Q. Wu, “Adaptive tracking control of mobile manipulators

with affine constraints and under-actuated joints,” Int. J. Autom. Comput., no. 2, pp. 1–8,

2016.

[53] University at Buffalo, “Cooperative payload transport by robot collectives.” [Online].

Available: http://mechatronics.eng.buffalo.edu/research/mobilemanipulator/index.html.

[54] “Dept of Precision Engineering, University of Tokyo.” [Online]. Available:

http://www.race.u-tokyo.ac.jp/otalab/research.htm.

[55] “MR helper.” [Online]. Available: http://www.plasticpals.com/?p=24409.

[56] E. and E. E. Department of Mechanical, “Shimane University.” [Online]. Available:

http://www.ecs.shimane-u.ac.jp/~hamaguchi/robotics/study-e.html.

[57] S. G. Tzafestas, Introduction to mobile robot control. 2014.

[58] T. Yoshikawa, “Manipulability of robotic mechanisms,” Int. J. Rob. Res., vol. 4, no. 2, pp.

3–9, 1985.

124

[59] M. Mustafa, A. Ramirez-serrano, K. A. Davies, and G. N. Wilson, “Modeling and

autonomous control of multiple mobile manipulators handling rigid objects,” pp. 397–406,

2012.

[60] Z. Li, S. S. Ge, and Z. Wang, “Robust adaptive control of coordinated multiple mobile

manipulators,” no. October, pp. 1–3, 2007.

[61] Y. Kanayama, “A stable tracking control method for an autonomous mobile robot,” IEEE

Int. Conf. Robot. Autom., vol. 1, no. May, pp. 384–389, 1990.

[62] N. York, “Backstepping based multiple mobile robots formation.”

[63] R. Fierro and F. Lewis, “Control of a nonholonomic mobile robot: backstepping

kinematics into dynamics,” Proc. 1995 34th IEEE Conf. Decis. Control, no. December,

pp. 3805–3810, 1995.

[64] Albus James S., Brains, behavior, and robotics,. BYTE Publications Inc, 1981.

[65] J. Albus, “A new approach to manipulator control: The cerebellar model articulation

controller (CMAC),” J. Dyn. Syst. Meas. Control, no. SEPTEMBER, pp. 220–227, 1975.

[66] J. S. Albus, “Data storage in the Cerebellar Model Articulation Controller (CMAC),”

Journal of Dynamic Systems, Measurement, and Control, vol. 97, no. 3. p. 228, 1975.

Modeling and Dynamic Control of Autonomous Ground Mobile ...

Documents

Transcript of Modeling and Dynamic Control of Autonomous Ground Mobile ...